diff --git a/conf/messages.xml b/conf/messages.xml index 3225bd0fa7..608c3942fe 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -66,7 +66,8 @@ - + + @@ -298,9 +299,9 @@ - - - + + + @@ -315,7 +316,7 @@ - + diff --git a/sw/airborne/autopilot/main.c b/sw/airborne/autopilot/main.c index 89adf17e81..34f0d99cc9 100644 --- a/sw/airborne/autopilot/main.c +++ b/sw/airborne/autopilot/main.c @@ -224,7 +224,7 @@ uint8_t ac_ident = AC_ID; /** \def EventPos(_cpt, _channel, _event) * @@@@@ A FIXER @@@@@ */ -#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); +#define PERIODIC_SEND_PPRZ_MODE() DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &lateral_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); #define PERIODIC_SEND_DESIRED() DOWNLINK_SEND_DESIRED(&desired_roll, &desired_pitch, &desired_x, &desired_y, &desired_altitude, &desired_climb); #define PERIODIC_SEND_NAVIGATION_REF() DOWNLINK_SEND_NAVIGATION_REF(&nav_utm_east0, &nav_utm_north0, &nav_utm_zone0); @@ -324,7 +324,7 @@ inline void radio_control_task( void ) { } mode_changed |= mcu1_status_update(); if ( mode_changed ) - DOWNLINK_SEND_PPRZ_MODE(&pprz_mode, &vertical_mode, &inflight_calib_mode, &mcu1_status, &ir_estim_mode); + PERIODIC_SEND_PPRZ_MODE(); /** If Auto1 mode, compute \a desired_roll and \a desired_pitch from * \a RADIO_ROLL and \a RADIO_PITCH \n diff --git a/sw/ground_segment/cockpit/Paparazzi/PFD.pm b/sw/ground_segment/cockpit/Paparazzi/PFD.pm index 461f945462..253a485d3a 100644 --- a/sw/ground_segment/cockpit/Paparazzi/PFD.pm +++ b/sw/ground_segment/cockpit/Paparazzi/PFD.pm @@ -33,7 +33,7 @@ sub populate { -alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], -target_alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], -heading => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], - -target_heading => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + -target_course => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], -vz => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], -ap_mode => [S_NOINIT, S_PRPGONLY, S_RDWR, S_OVRWRT, S_CHILDREN, 0], @@ -63,7 +63,7 @@ sub selected_ac { # print "###########\n"; # print "in PFD selected_ac $previous_ac $new_ac\n"; - my @fields = ('roll', 'pitch', 'speed', 'alt', 'target_alt', 'target_heading', 'gps_mode', 'ap_mode'); + my @fields = ('roll', 'pitch', 'speed', 'alt', 'target_alt', 'target_course', 'gps_mode', 'ap_mode'); foreach my $field ( @fields ) { $previous_ac->detach($self, $field, [\&foo_cbk, $field]) if ($previous_ac); @@ -177,7 +177,7 @@ sub build_gui() { -repeat_legend => 3, ); $self->connectoptions(-heading, S_TO, [$self->{heading_scale}, -value]); - $self->connectoptions(-target_heading, S_TO, [$self->{heading_scale}, -target_value]); + $self->connectoptions(-target_course, S_TO, [$self->{heading_scale}, -target_value]); $self->{alt_scale} = Paparazzi::LensScale->new( -zinc => $zinc, -parent_grp => $self->{main_group}, -origin => [0.8*$width, 0.25*$height], diff --git a/sw/ground_segment/tmtc/receive.ml b/sw/ground_segment/tmtc/receive.ml index 3139fb5ec6..6268b3a67a 100644 --- a/sw/ground_segment/tmtc/receive.ml +++ b/sw/ground_segment/tmtc/receive.ml @@ -41,6 +41,21 @@ let logs_path = Env.paparazzi_home // "var" // "logs" let conf_xml = Xml.parse_file (Env.paparazzi_home // "conf" // "conf.xml") let srtm_path = Env.paparazzi_home // "data" // "srtm" +(** Should be read from messages.xml *) +let ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME"|] +let gaz_modes = [|"MANUAL";"GAZ";"CLIMB";"ALT"|] +let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|] + +let check_index = fun i t where -> + if i < 0 || i >= Array.length t then begin + Debug.call 'E' (fun f -> fprintf f "Wrong index in %s: %d" where i); + -1 + end else + i + +let get_indexed_value = fun t i -> + if i >= 0 then t.(i) else "UNK" + type ac_cam = { mutable phi : float; (* Rad, right = >0 *) mutable theta : float; (* Rad, front = >0 *) @@ -97,6 +112,8 @@ type aircraft = { mutable desired_east : float; mutable desired_north : float; mutable desired_altitude : float; + mutable desired_course : float; + mutable desired_climb : float; mutable gspeed : float; mutable course : float; mutable alt : float; @@ -110,14 +127,17 @@ type aircraft = { mutable amp : float; mutable energy : float; mutable ap_mode : int; - mutable ap_altitude : int; + mutable gaz_mode : int; + mutable lateral_mode : int; cam : ac_cam; mutable gps_mode : int; inflight_calib : inflight_calib; infrared : infrared; fbw : fbw; svinfo : svinfo array; - mutable flight_time : int + mutable flight_time : int; + mutable stage_time : int; + mutable block_time : int } (** The aircrafts store *) @@ -176,7 +196,8 @@ let log_and_parse = fun log ac_name a msg values -> | "DESIRED" -> a.desired_east <- fvalue "desired_x"; a.desired_north <- fvalue "desired_y"; - a.desired_altitude <- fvalue "desired_altitude" + a.desired_altitude <- fvalue "desired_altitude"; + a.desired_climb <- fvalue "desired_climb" | "NAVIGATION_REF" -> a.nav_ref_east <- fvalue "utm_east"; a.nav_ref_north <- fvalue "utm_north" @@ -185,15 +206,19 @@ let log_and_parse = fun log ac_name a msg values -> a.pitch <- (Deg>>Rad) (fvalue "theta") | "NAVIGATION" -> a.cur_block <- ivalue "cur_block"; - a.cur_stage <- ivalue "cur_stage" + a.cur_stage <- ivalue "cur_stage"; + a.desired_course <- fvalue "desired_course" /. 10. | "BAT" -> a.throttle <- fvalue "desired_gaz" /. 9600. *. 100.; a.flight_time <- ivalue "flight_time"; a.rpm <- a.throttle *. 100.; - a.bat <- fvalue "voltage" /. 10. + a.bat <- fvalue "voltage" /. 10.; + a.stage_time <- ivalue "stage_time"; + a.block_time <- ivalue "block_time" | "PPRZ_MODE" -> - a.ap_mode <- ivalue "ap_mode"; - a.ap_altitude <- ivalue "ap_altitude"; + a.ap_mode <- check_index (ivalue "ap_mode") ap_modes "AP_MODE"; + a.gaz_mode <- check_index (ivalue "ap_gaz") gaz_modes "AP_GAZ"; + a.lateral_mode <- check_index (ivalue "ap_lateral") lat_modes "AP_LAT"; a.inflight_calib.if_mode <- ivalue "if_calib_mode"; let mcu1_status = ivalue "mcu1_status" in (** c.f. link_autopilot.h *) @@ -322,9 +347,14 @@ let send_aircraft_msg = fun ac -> let values = ["ac_id", Pprz.String ac; "cur_block", Pprz.Int a.cur_block; "cur_stage", Pprz.Int a.cur_stage; + "stage_time", Pprz.Int a.stage_time; + "block_time", Pprz.Int a.block_time; "target_east", f (a.nav_ref_east+.a.desired_east); "target_north", f (a.nav_ref_north+.a.desired_north); - "target_alt", Pprz.Float a.desired_altitude] in + "target_alt", Pprz.Float a.desired_altitude; + "target_climb", Pprz.Float a.desired_climb; + "target_course", Pprz.Float a.desired_course + ] in Ground_Pprz.message_send my_id "NAV_STATUS" values; let values = ["ac_id", Pprz.String ac; @@ -335,11 +365,15 @@ let send_aircraft_msg = fun ac -> "amp", f a.amp; "energy", f a.energy] in Ground_Pprz.message_send my_id "ENGINE_STATUS" values; - + + let ap_mode = get_indexed_value ap_modes a.ap_mode in + let gaz_mode = get_indexed_value gaz_modes a.gaz_mode in + let lat_mode = get_indexed_value lat_modes a.lateral_mode in let values = ["ac_id", Pprz.String ac; "flight_time", Pprz.Int a.flight_time; - "ap_mode", Pprz.Int a.ap_mode; - "v_mode", Pprz.Int a.ap_altitude; + "ap_mode", Pprz.String ap_mode; + "gaz_mode", Pprz.String gaz_mode; + "lat_mode", Pprz.String lat_mode; "gps_mode", Pprz.Int a.gps_mode] in Ground_Pprz.message_send my_id "AP_STATUS" values; @@ -353,15 +387,20 @@ let send_aircraft_msg = fun ac -> | x -> prerr_endline (Printexc.to_string x) let new_aircraft = fun id -> - { id = id ; roll = 0.; pitch = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode=0; ap_altitude=0; gps_mode =0; + { id = id ; roll = 0.; pitch = 0.; nav_ref_east = 0.; nav_ref_north = 0.; desired_east = 0.; desired_north = 0.; + desired_course = 0.; + gspeed=0.; course = 0.; alt=0.; climb=0.; cur_block=0; cur_stage=0; throttle = 0.; rpm = 0.; temp = 0.; bat = 0.; amp = 0.; energy = 0.; ap_mode= -1; + gaz_mode= -1; lateral_mode= -1; + gps_mode =0; desired_altitude = 0.; + desired_climb = 0.; pos = { utm_x = 0.; utm_y = 0.; utm_zone = 0 }; cam = { phi = 0.; theta = 0. }; inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.}; infrared = infrared_init; fbw = { rc_status = "???"; rc_mode = "???" }; svinfo = Array.create gps_nb_channels svinfo_init; - flight_time = 0; + flight_time = 0; stage_time = 0; block_time = 0 } let register_aircraft = fun name a -> diff --git a/sw/lib/perl/Paparazzi/Aircraft.pm b/sw/lib/perl/Paparazzi/Aircraft.pm index 893dbc25fa..ffeda8c7f0 100644 --- a/sw/lib/perl/Paparazzi/Aircraft.pm +++ b/sw/lib/perl/Paparazzi/Aircraft.pm @@ -49,11 +49,11 @@ sub populate { climb => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], ap_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], - h_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], - v_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + lat_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + gaz_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], target_climb => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], target_alt => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], - target_heading=> [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], + target_course=> [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], gps_mode => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.], cur_block => [S_NOINIT, S_PASSIVE, S_RDWR, S_OVRWRT, S_NOPRPG, 0.],