diff --git a/conf/airframes/microjet7.xml b/conf/airframes/microjet7.xml index 1874dfafd7..642e1ded32 100644 --- a/conf/airframes/microjet7.xml +++ b/conf/airframes/microjet7.xml @@ -5,8 +5,12 @@ - - + + + @@ -102,8 +106,8 @@ - - + + @@ -249,9 +253,10 @@ ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c ap.CFLAGS += -DGYRO -DADXRS150 -ap.srcs += gyro.c +ap.srcs += gyro.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c +ap.srcs += traffic_info.c -# ap.srcs += bomb.c +ap.srcs += bomb.c # Hack to use the same tuning file than slayer1 ap.CFLAGS += -DUSE_GPIO @@ -267,7 +272,7 @@ ap.srcs += $(SRC_ARCH)/gpio.c include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DH_CTL_RATE_LOOP -DLOITER_TRIM -DALT_KALMAN -DIR_360 sim.srcs += traffic_info.c -sim.srcs += anemotaxis.c chemotaxis.c discsurvey.c nav_line.c +sim.srcs += bomb.c nav_line.c chemotaxis.c anemotaxis.c discsurvey.c diff --git a/conf/airframes/minimag1.xml b/conf/airframes/minimag1.xml index 06a3ab6bf8..f256b2bfd4 100644 --- a/conf/airframes/minimag1.xml +++ b/conf/airframes/minimag1.xml @@ -4,7 +4,7 @@ - + @@ -12,7 +12,7 @@ - + @@ -59,18 +59,18 @@
- + - - + + - - + + @@ -117,7 +117,7 @@ - + @@ -144,7 +144,7 @@ - + diff --git a/conf/flight_plans/IS.xml b/conf/flight_plans/IS.xml index 9bb2929d79..2837a10e2f 100644 --- a/conf/flight_plans/IS.xml +++ b/conf/flight_plans/IS.xml @@ -1,22 +1,29 @@ - +
#include "anemotaxis.h" #include "chemotaxis.h" #include "discsurvey.h" +#include "nav_line.h" +#include "bomb.h"
- - + + - + + + + + + - + @@ -39,6 +46,12 @@ + + + + + + @@ -48,21 +61,28 @@ - - + + - - + + + + + + + + + - + @@ -83,7 +103,7 @@ - + @@ -113,5 +133,26 @@ + + + + + + + + + + + + + + + + + + + + +
diff --git a/conf/flight_plans/versatile.xml b/conf/flight_plans/versatile.xml index 24c178426e..109c775196 100644 --- a/conf/flight_plans/versatile.xml +++ b/conf/flight_plans/versatile.xml @@ -1,6 +1,9 @@ - + +
+#include "bomb.h" +
@@ -82,8 +85,11 @@ + +
diff --git a/sw/airborne/anemotaxis.c b/sw/airborne/anemotaxis.c index e27e10f9e1..4a77868986 100644 --- a/sw/airborne/anemotaxis.c +++ b/sw/airborne/anemotaxis.c @@ -17,10 +17,10 @@ static void last_plume_was_here( void ) { last_plume.y = estimator_y; } -bool_t nav_anemotaxis_downwind( uint8_t c ) { +bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) { float wind_dir = atan2(wind_north, wind_east); - waypoints[c].x = waypoints[WP_HOME].x + MAX_DIST_FROM_HOME*0.9*cos(wind_dir); - waypoints[c].y = waypoints[WP_HOME].y + MAX_DIST_FROM_HOME*0.9*sin(wind_dir); + waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir); + waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir); return FALSE; } @@ -39,7 +39,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { last_plume_was_here(); waypoints[plume].x = estimator_x; waypoints[plume].y = estimator_y; - DownlinkSendWp(plume); + // DownlinkSendWp(plume); } float wind_dir = atan2(wind_north, wind_east) + M_PI; @@ -62,8 +62,8 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign; waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign; - DownlinkSendWp(c1); - DownlinkSendWp(c2); + // DownlinkSendWp(c1); + // DownlinkSendWp(c2); status = CROSSWIND; nav_init_stage(); @@ -76,7 +76,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { waypoints[c].x = waypoints[c2].x + MIN_CIRCLE_RADIUS*upwind_x; waypoints[c].y = waypoints[c2].y + MIN_CIRCLE_RADIUS*upwind_y; - DownlinkSendWp(c); + // DownlinkSendWp(c); sign = -sign; status = UTURN; @@ -87,7 +87,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { waypoints[c].x = estimator_x + MIN_CIRCLE_RADIUS*upwind_x; waypoints[c].y = estimator_y + MIN_CIRCLE_RADIUS*upwind_y; - DownlinkSendWp(c); + // DownlinkSendWp(c); sign = -sign; status = UTURN; diff --git a/sw/airborne/anemotaxis.h b/sw/airborne/anemotaxis.h index dfda8f80eb..05f7d52e57 100644 --- a/sw/airborne/anemotaxis.h +++ b/sw/airborne/anemotaxis.h @@ -1,7 +1,7 @@ #include "std.h" extern uint8_t chemo_sensor; -extern bool_t nav_anemotaxis_downwind(uint8_t c); +extern bool_t nav_anemotaxis_downwind(uint8_t c, float radius); extern bool_t nav_anemotaxis_init(uint8_t c); extern bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume); diff --git a/sw/airborne/chemotaxis.c b/sw/airborne/chemotaxis.c index 98ccd0adf4..237506d3f8 100644 --- a/sw/airborne/chemotaxis.c +++ b/sw/airborne/chemotaxis.c @@ -7,40 +7,48 @@ uint8_t chemo_sensor; -#define MAX_RADIUS 500 +#define MAX_RADIUS 250 #define ALPHA 0.5 static uint8_t last_plume_value; +#define MAX_CHEMO 255 static float radius; static int8_t sign; -bool_t nav_chemotaxis_init( void ) { +bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ) { radius = MAX_RADIUS; last_plume_value = 0; sign = 1; + waypoints[plume].x = waypoints[c].x; + waypoints[plume].y = waypoints[c].y; + chemo_sensor = 0; return FALSE; } bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) { if (chemo_sensor > last_plume_value) { + /* Move the circle in this direction */ + float x = estimator_x - waypoints[plume].x; + float y = estimator_y - waypoints[plume].y; + waypoints[c].x = waypoints[plume].x + ALPHA * x; + waypoints[c].y = waypoints[plume].y + ALPHA * y; + // DownlinkSendWp(c); + /* Turn in the right direction */ + float dir_x = cos(M_PI_2 - estimator_hspeed_dir); + float dir_y = sin(M_PI_2 - estimator_hspeed_dir); + float pvect = dir_x*y - dir_y*x; + sign = (pvect > 0 ? -1 : 1); + /* Reduce the radius */ + radius = sign * (MIN_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-MIN_CIRCLE_RADIUS)); + + /* Store this plume */ waypoints[plume].x = estimator_x; waypoints[plume].y = estimator_y; - DownlinkSendWp(plume); + // DownlinkSendWp(plume); last_plume_value = chemo_sensor; - - /* Reduce the radius */ - sign = - sign; - radius = sign * (MIN_CIRCLE_RADIUS+(255-chemo_sensor)/256.*(MAX_RADIUS-MIN_CIRCLE_RADIUS)); - - /* Move the circle in this direction */ - float x = waypoints[c].x - waypoints[plume].x; - float y = waypoints[c].y - waypoints[plume].y; - waypoints[c].x = waypoints[plume].x + ALPHA * x; - waypoints[c].y = waypoints[plume].y + ALPHA * y; - DownlinkSendWp(c); } NavCircleWaypoint(c, radius); diff --git a/sw/airborne/chemotaxis.h b/sw/airborne/chemotaxis.h index b3fc5b8764..9129285e2c 100644 --- a/sw/airborne/chemotaxis.h +++ b/sw/airborne/chemotaxis.h @@ -1,4 +1,4 @@ #include "std.h" -extern bool_t nav_chemotaxis_init( void ); +extern bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ); extern bool_t nav_chemotaxis( uint8_t c, uint8_t plume ); diff --git a/sw/airborne/discsurvey.c b/sw/airborne/discsurvey.c index 054ae394f7..37204714e1 100644 --- a/sw/airborne/discsurvey.c +++ b/sw/airborne/discsurvey.c @@ -5,24 +5,22 @@ #include "flight_plan.h" #include "ap_downlink.h" -enum status { UTURN, CROSSWIND }; +enum status { UTURN, SEGMENT, DOWNWIND }; static enum status status; static int8_t sign; -static int8_t upwind; static struct point c; static struct point c1; static struct point c2; bool_t disc_survey_init( void ) { - status = UTURN; + status = DOWNWIND; sign = 1; - c.x = estimator_x; - c.y = estimator_y; - upwind = 1; + c1.x = estimator_x; + c1.y = estimator_y; return FALSE; } -bool_t disc_survey( uint8_t center, float radius ) { +bool_t disc_survey( uint8_t center, float radius, float grid ) { float wind_dir = atan2(wind_north, wind_east) + M_PI; /** Not null even if wind_east=wind_north=0 */ @@ -31,30 +29,40 @@ bool_t disc_survey( uint8_t center, float radius ) { switch (status) { case UTURN: - nav_circle_XY(c.x, c.y, MIN_CIRCLE_RADIUS*sign); + nav_circle_XY(c.x, c.y, grid*sign); if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { c1.x = estimator_x; c1.y = estimator_y; float d = ScalarProduct(upwind_x, upwind_y, estimator_x-waypoints[center].x, estimator_y-waypoints[center].y); - float w = sqrt(radius*radius - d*d) - 1.5*MIN_CIRCLE_RADIUS; - - float crosswind_x = - upwind_y; - float crosswind_y = upwind_x; - - c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x; - c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y; - - status = CROSSWIND; + if (d > radius) { + status = DOWNWIND; + } else { + float w = sqrt(radius*radius - d*d) - 1.5*grid; + + float crosswind_x = - upwind_y; + float crosswind_y = upwind_x; + + c2.x = waypoints[center].x+d*upwind_x-w*sign*crosswind_x; + c2.y = waypoints[center].y+d*upwind_y-w*sign*crosswind_y; + + status = SEGMENT; + } nav_init_stage(); } break; - case CROSSWIND: + case DOWNWIND: + c2.x = waypoints[center].x - upwind_x * radius; + c2.y = waypoints[center].y - upwind_y * radius; + status = SEGMENT; + /* No break; */ + + case SEGMENT: nav_route_xy(c1.x, c1.y, c2.x, c2.y); if (nav_approaching_xy(c2.x, c2.y, CARROT)) { - c.x = c2.x + MIN_CIRCLE_RADIUS*upwind_x; - c.y = c2.y + MIN_CIRCLE_RADIUS*upwind_y; + c.x = c2.x + grid*upwind_x; + c.y = c2.y + grid*upwind_y; sign = -sign; status = UTURN; @@ -62,5 +70,9 @@ bool_t disc_survey( uint8_t center, float radius ) { } break; } + + NavVerticalAutoThrottleMode(0.); /* No pitch */ + NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */ + return TRUE; } diff --git a/sw/airborne/discsurvey.h b/sw/airborne/discsurvey.h index d680f722ca..df90526f75 100644 --- a/sw/airborne/discsurvey.h +++ b/sw/airborne/discsurvey.h @@ -1,5 +1,5 @@ #include "std.h" extern bool_t disc_survey_init( void ); -extern bool_t disc_survey(uint8_t c, float radius); +extern bool_t disc_survey(uint8_t c, float radius, float grid); diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index bc2d97f024..5ab3bab19c 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -158,40 +158,6 @@ void nav_circle_XY(float x, float y, float radius) { #define NavSurveyRectangleInit(_wp1, _wp2, _grid) nav_survey_rectangle_init(_wp1, _wp2, _grid) #define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2) -/** Set the climb control to auto-throttle with the specified pitch - pre-command */ -#define NavVerticalAutoThrottleMode(_pitch) { \ - v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \ - nav_pitch = _pitch; \ -} - -/** Set the climb control to auto-pitch with the specified throttle - pre-command */ -#define NavVerticalAutoPitchMode(_throttle) { \ - v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \ - nav_throttle_setpoint = _throttle; \ -} - -/** Set the vertical mode to altitude control with the specified altitude - setpoint and climb pre-command. */ -#define NavVerticalAltitudeMode(_alt, _pre_climb) { \ - v_ctl_mode = V_CTL_MODE_AUTO_ALT; \ - nav_altitude = _alt; \ - v_ctl_altitude_pre_climb = _pre_climb; \ -} - -/** Set the vertical mode to climb control with the specified climb setpoint */ -#define NavVerticalClimbMode(_climb) { \ - v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \ - v_ctl_climb_setpoint = _climb; \ -} - -/** Set the vertical mode to fixed throttle with the specified setpoint */ -#define NavVerticalThrottleMode(_throttle) { \ - v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \ - nav_throttle_setpoint = _throttle; \ -} - #define NavGlide(_last_wp, _wp) { \ float start_alt = waypoints[_last_wp].a; \ float diff_alt = waypoints[_wp].a - start_alt; \ @@ -431,7 +397,7 @@ static inline void fly_to_xy(float x, float y) { 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; - float leg2 = leg_x * leg_x + leg_y * leg_y; + float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); nav_leg_progress = ((estimator_x - last_wp_x) * leg_x + (estimator_y - last_wp_y) * leg_y) / leg2; nav_leg_progress = Max(nav_leg_progress, 0.); nav_leg_length = sqrt(leg2); diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index fa747fa94c..00961cbc22 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -35,6 +35,7 @@ #include "std.h" #include "paparazzi.h" #include "airframe.h" +#include "fw_v_ctl.h" #define G 9.806 #define Square(_x) ((_x)*(_x)) @@ -151,4 +152,40 @@ extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_ bool_t nav_approaching_xy(float x, float y, float approaching_time); #define NavApproaching(wp, time) nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, time) +/** Set the climb control to auto-throttle with the specified pitch + pre-command */ +#define NavVerticalAutoThrottleMode(_pitch) { \ + v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \ + nav_pitch = _pitch; \ +} + +/** Set the climb control to auto-pitch with the specified throttle + pre-command */ +#define NavVerticalAutoPitchMode(_throttle) { \ + v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \ + nav_throttle_setpoint = _throttle; \ +} + +/** Set the vertical mode to altitude control with the specified altitude + setpoint and climb pre-command. */ +#define NavVerticalAltitudeMode(_alt, _pre_climb) { \ + v_ctl_mode = V_CTL_MODE_AUTO_ALT; \ + nav_altitude = _alt; \ + v_ctl_altitude_pre_climb = _pre_climb; \ +} + +/** Set the vertical mode to climb control with the specified climb setpoint */ +#define NavVerticalClimbMode(_climb) { \ + v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \ + v_ctl_climb_setpoint = _climb; \ +} + +/** Set the vertical mode to fixed throttle with the specified setpoint */ +#define NavVerticalThrottleMode(_throttle) { \ + v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \ + nav_throttle_setpoint = _throttle; \ +} + + + #endif /* NAV_H */ diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index dc3fdc6708..dec2e22c1a 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -70,21 +70,32 @@ type status = { mutable rx_byte : int; mutable rx_msg : int; mutable rx_err : int; + mutable ms_since_last_msg : int } let statuss = Hashtbl.create 3 +let dead_aircraft_time_ms = 5000 let update_status = fun ac_id buf -> let status = try Hashtbl.find statuss ac_id with Not_found -> - let s = { last_rx_byte = 0; last_rx_msg = 0; rx_byte = 0; rx_msg = 0; rx_err = 0 } in + let s = { last_rx_byte = 0; last_rx_msg = 0; rx_byte = 0; rx_msg = 0; rx_err = 0; ms_since_last_msg = dead_aircraft_time_ms } in Hashtbl.add statuss ac_id s; s in status.rx_byte <- status.rx_byte + String.length buf; status.rx_msg <- status.rx_msg + 1; - status.rx_err <- !PprzTransport.nb_err + status.rx_err <- !PprzTransport.nb_err; + status.ms_since_last_msg <- 0 let status_msg_period = 1000 (** ms *) + +let live_aircraft = fun ac_id -> + try + let s = Hashtbl.find statuss ac_id in + s.ms_since_last_msg < dead_aircraft_time_ms + with + Not_found -> false + let send_status_msg = let start = Unix.gettimeofday () in fun () -> @@ -95,6 +106,7 @@ let send_status_msg = and msg_rate = float (status.rx_msg - status.last_rx_msg) /. dt in status.last_rx_msg <- status.rx_msg; status.last_rx_byte <- status.rx_byte; + status.ms_since_last_msg <- status.ms_since_last_msg + status_msg_period; let vs = ["run_time", Pprz.Int t; "rx_bytes_rate", Pprz.Float byte_rate; "rx_msgs_rate", Pprz.Float msg_rate; @@ -117,7 +129,6 @@ let airframes = let device = get_define dls "DEVICE_TYPE" and addr = get_define dls "DEVICE_ADDRESS" in let dl = airborne_device device addr in - printf "%s %b\n%!" (ExtXml.attrib a "ac_id") (dl = Uart); (ios (ExtXml.attrib a "ac_id"), dl)::r with Not_found -> r @@ -383,7 +394,7 @@ let get_fp = fun device _sender vs -> let ac_id = int_of_string (Pprz.string_assoc "ac_id" vs) in List.iter (fun (dest_id, _) -> - if dest_id <> ac_id then (** Do not send to itself *) + if dest_id <> ac_id && live_aircraft dest_id then (** Do not send to itself *) try Debug.trace 'b' (sprintf "ACINFO %d for %d" ac_id dest_id); let ac_device = airborne_device dest_id airframes device.transport in @@ -613,7 +624,7 @@ let _ = if !uplink then begin (** Listening on Ivy (FIXME: remove the ad hoc messages) *) - ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device)); +(***) ignore (Ground_Pprz.message_bind "FLIGHT_PARAM" (get_fp device)); (***) ignore (Ground_Pprz.message_bind "MOVE_WAYPOINT" (move_wp device)); ignore (Ground_Pprz.message_bind "DL_SETTING" (setting device)); ignore (Ground_Pprz.message_bind "JUMP_TO_BLOCK" (jump_block device)); diff --git a/sw/simulator/diffusion.ml b/sw/simulator/diffusion.ml index 9b6095bb0e..e1dc1feae9 100644 --- a/sw/simulator/diffusion.ml +++ b/sw/simulator/diffusion.ml @@ -8,7 +8,7 @@ type plume = { mutable utm_x : float; mutable utm_y : float; mutable value : int (* NW of Muret ref *) let muret = utm_of WGS84 {LL.posn_lat=(Deg>>Rad)43.4624; posn_long=(Deg>>Rad)1.2727} -let source = fun () -> { utm_x = muret.LL.utm_x -. 500.; utm_y = muret.LL.utm_y +. 500.; value = 255} +let source = fun () -> { utm_x = muret.LL.utm_x -. 275.; utm_y = muret.LL.utm_y +. 275.; value = 255} let available_ids = ref [] let gen_id = diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 7e0500478c..8e8ca4a87d 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -126,7 +126,7 @@ let get_index_waypoint = fun x l -> try string_of_int (List.assoc x l) with - Not_found -> failwith (sprintf "Unknown waypoint: %s\n" x) + Not_found -> failwith (sprintf "Unknown waypoint: %s" x) let output_cam_mode = fun x index_of_waypoints -> let m = try Xml.attrib x "cam_mode" with _ -> "fix" in @@ -336,7 +336,7 @@ let rec print_stage = fun index_of_waypoints x -> try get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints with - _ -> + ExtXml.Error _ -> lprintf "waypoints[0].x = %s;\n" (parsed_attrib x "x"); lprintf "waypoints[0].y = %s;\n" (parsed_attrib x "y"); "0" @@ -747,4 +747,5 @@ let _ = | Dtd.Prove_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.prove_error e); exit 1 | Dtd.Check_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.check_error e); exit 1 | Dtd.Parse_error e -> fprintf stderr "%s: DTD error:%s\n%!" !xml_file (Dtd.parse_error e); exit 1 + | Failure x -> fprintf stderr "%s: %s\n" !xml_file x; exit 1