mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 03:57:45 +08:00
lots of new bugs
This commit is contained in:
Pascal Brisset
+6
-5
@@ -66,7 +66,8 @@
|
||||
|
||||
<message name="PPRZ_MODE" ID="11" period="5" >
|
||||
<field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME"/>
|
||||
<field name="ap_altitude" type="uint8" values="MANUAL|AUTO_GAZ|AUTO_CLIMB|AUTO_ALT"/>
|
||||
<field name="ap_gaz" type="uint8" values="MANUAL|AUTO_GAZ|AUTO_CLIMB|AUTO_ALT"/>
|
||||
<field name="ap_lateral" type="uint8" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
|
||||
<field name="if_calib_mode" type="uint8" values="NONE|DOWN|UP"/>
|
||||
<field name="mcu1_status" type="uint8" values="LOST|OK|REALLY_LOST"/>
|
||||
<field name="lls_calib" type="uint8" values="LLS_CALIB_MODE_OFF|LLS_CALIB_MODE_ON"/>
|
||||
@@ -298,9 +299,9 @@
|
||||
|
||||
<message name="AP_STATUS" ID="12">
|
||||
<field name="ac_id" type="string"/>
|
||||
<field name="ap_mode" type="uint8" values="MANUAL|AUTO1|AUTO2|HOME"/>
|
||||
<field name="h_mode" type="uint8" values="NONE|ROLLDOT|ROLL|HEADING_DOT|HEADING|WP|ROUTE|CIRCLE"/>
|
||||
<field name="v_mode" type="uint8" values="NONE|CLIMB|ALT|GLIDE|PARABOLIC"/>
|
||||
<field name="ap_mode" type="string" values="MANUAL|AUTO1|AUTO2|HOME"/>
|
||||
<field name="lat_mode" type="string" values="MANUAL|ROLL_RATE|ROLL|COURSE"/>
|
||||
<field name="gaz_mode" type="string" values="MANUAL|GAZ|CLIMB|ALT"/>
|
||||
<field name="gps_mode" type="uint8"/>
|
||||
<field name="flight_time" type="uint32"/>
|
||||
</message>
|
||||
@@ -315,7 +316,7 @@
|
||||
<field name="target_north" type="float" unit="m"/>
|
||||
<field name="target_climb" type="float" unit="m/s"/>
|
||||
<field name="target_alt" type="float" unit="m"/>
|
||||
<field name="target_heading" type="float" unit="m"/>
|
||||
<field name="target_course" type="float" unit="deg"/>
|
||||
</message>
|
||||
|
||||
<message name="CAM_STATUS" ID="14">
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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],
|
||||
|
||||
@@ -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 ->
|
||||
|
||||
@@ -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.],
|
||||
|
||||
Reference in New Issue
Block a user