From 3fd4b18444e37fa526e3739d5b9923961bd99237 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 4 Nov 2014 16:39:27 +0100 Subject: [PATCH 01/86] [ground_segment] put google maps dir in Google subdir in var/maps should close #902 --- sw/lib/ocaml/gm.ml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/lib/ocaml/gm.ml b/sw/lib/ocaml/gm.ml index 83e70b125ba..ae4f7993bce 100644 --- a/sw/lib/ocaml/gm.ml +++ b/sw/lib/ocaml/gm.ml @@ -236,7 +236,7 @@ let url_of_tile_key = fun maps_source s -> let get_cache_dir = function - Google -> !cache_path (* Historic ! Should be // Google *) + Google -> !cache_path // "Google" | OSM -> !cache_path // "OSM" | MQ -> !cache_path // "MapQuest" | MQ_Aerial -> !cache_path // "MapQuestAerial" From 5fd3eb9b9ceb1a3f8cdbe60241bd3e5847f8c88b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 10 Nov 2014 20:10:30 +0100 Subject: [PATCH 02/86] [conf] example on how to use SPEECH_NAME --- conf/airframes/examples/quadrotor_navgo.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index a50a7b5cdab..1c8a2825a35 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -253,6 +253,7 @@ +
From 29a59d8d9630aeb894bdaf5244be5d0643a07b19 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 11 Nov 2014 15:15:29 +0100 Subject: [PATCH 03/86] add *.hgt files in srtm folder to gitignore closes #900 --- data/srtm/.gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/data/srtm/.gitignore b/data/srtm/.gitignore index cb2be8e01a8..784bb962bc6 100644 --- a/data/srtm/.gitignore +++ b/data/srtm/.gitignore @@ -1,2 +1,3 @@ *.hgt.zip *.hgt.bz2 +*.hgt From b44051c23c9b57ad599aba93eaca52bcfbd49d5f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 11 Nov 2014 16:31:02 +0100 Subject: [PATCH 04/86] [simulator] NPS: add --time_factor command line option --- sw/simulator/nps/nps_main.c | 9 ++++++--- sw/simulator/pprzsim-launch | 5 +++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index a920021cc20..e39633aadd6 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -39,7 +39,6 @@ #define SIM_DT (1./SYS_TIME_FREQUENCY) #define DISPLAY_DT (1./30.) #define HOST_TIMEOUT_MS 40 -#define HOST_TIME_FACTOR 1. static struct { double real_initial_time; @@ -117,7 +116,6 @@ static void nps_main_init(void) { gettimeofday (&t, NULL); nps_main.real_initial_time = time_to_double(&t); nps_main.scaled_initial_time = time_to_double(&t); - nps_main.host_time_factor = HOST_TIME_FACTOR; nps_ivy_init(nps_main.ivy_bus); nps_fdm_init(SIM_DT); @@ -257,6 +255,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) { nps_main.spektrum_dev = NULL; nps_main.rc_script = 0; nps_main.ivy_bus = NULL; + nps_main.host_time_factor = 1.0; static const char* usage = "Usage: %s [options]\n" @@ -268,7 +267,8 @@ static bool_t nps_main_parse_options(int argc, char** argv) { " -j --js_dev e.g. 1 (default 0)\n" " --spektrum_dev e.g. /dev/ttyUSB0\n" " --rc_script e.g. 0\n" -" --ivy_bus e.g. 127.255.255.255\n"; +" --ivy_bus e.g. 127.255.255.255\n" +" --time_factor e.g. 2.5\n"; while (1) { @@ -281,6 +281,7 @@ static bool_t nps_main_parse_options(int argc, char** argv) { {"spektrum_dev", 1, NULL, 0}, {"rc_script", 1, NULL, 0}, {"ivy_bus", 1, NULL, 0}, + {"time_factor", 1, NULL, 0}, {0, 0, 0, 0} }; int option_index = 0; @@ -308,6 +309,8 @@ static bool_t nps_main_parse_options(int argc, char** argv) { nps_main.rc_script = atoi(optarg); break; case 6: nps_main.ivy_bus = strdup(optarg); break; + case 7: + nps_main.host_time_factor = atof(optarg); break; } break; diff --git a/sw/simulator/pprzsim-launch b/sw/simulator/pprzsim-launch index f6d953dd413..e9f87e5e7e7 100755 --- a/sw/simulator/pprzsim-launch +++ b/sw/simulator/pprzsim-launch @@ -76,6 +76,8 @@ def main(): help="Spektrum device to use (e.g. /dev/ttyUSB0)") nps_opts.add_option("--rc_script", type="int", action="store", metavar="NO", help="Number of RC script to use") + nps_opts.add_option("--time_factor", type="float", action="store", metavar="factor", + help="Time factor (default 1.0)") # special options for the JSBSim sim (without sensor models) #jsbsim_opts = OptionGroup(parser, "JSBSim options", @@ -138,6 +140,9 @@ def main(): if options.ivy_bus: simargs.append("--ivy_bus") simargs.append(options.ivy_bus) + if options.time_factor: + simargs.append("--time_factor") + simargs.append(str(options.time_factor)) else: parser.error("Please specify a valid sim type.") From 70feaf1455d5e212d75c14a02ff5ffc215388991 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 11 Nov 2014 18:33:31 +0100 Subject: [PATCH 05/86] [nps] always define USE_MISSION_COMMANDS_IN_NPS if mission_rotorcraft module is loaded --- conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile | 7 +------ conf/modules/mission_rotorcraft.xml | 3 +++ sw/simulator/nps/nps_ivy_mission_commands.c | 5 +++++ 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile index e19f469a337..c1f0e2f1035 100644 --- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile +++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile @@ -58,6 +58,7 @@ nps.srcs += $(NPSDIR)/nps_main.c \ $(NPSDIR)/nps_ivy_common.c \ $(NPSDIR)/nps_ivy_rotorcraft.c \ $(NPSDIR)/nps_flightgear.c \ + $(NPSDIR)/nps_ivy_mission_commands.c # for geo mag calculation nps.srcs += math/pprz_geodetic_wmm2010.c @@ -67,9 +68,3 @@ nps.srcs += subsystems/datalink/ivy_transport.c nps.srcs += subsystems/datalink/downlink.c subsystems/datalink/telemetry.c nps.srcs += $(SRC_FIRMWARE)/rotorcraft_telemetry.c nps.srcs += $(SRC_FIRMWARE)/datalink.c - -USE_MISSION_COMMANDS_IN_NPS ?= 0 -ifeq ($(USE_MISSION_COMMANDS_IN_NPS), 1) -nps.srcs += $(NPSDIR)/nps_ivy_mission_commands.c -nps.CFLAGS += -DUSE_MISSION_COMMANDS_IN_NPS -endif diff --git a/conf/modules/mission_rotorcraft.xml b/conf/modules/mission_rotorcraft.xml index fa5171d6c48..8aead7169aa 100644 --- a/conf/modules/mission_rotorcraft.xml +++ b/conf/modules/mission_rotorcraft.xml @@ -30,4 +30,7 @@ + + + diff --git a/sw/simulator/nps/nps_ivy_mission_commands.c b/sw/simulator/nps/nps_ivy_mission_commands.c index 715f33a4860..1e6e64416d7 100644 --- a/sw/simulator/nps/nps_ivy_mission_commands.c +++ b/sw/simulator/nps/nps_ivy_mission_commands.c @@ -1,3 +1,5 @@ +#if USE_MISSION_COMMANDS_IN_NPS + #include "nps_ivy.h" #include @@ -245,3 +247,6 @@ static void on_DL_END_MISSION(IvyClientPtr app __attribute__ ((unused)), mission_parse_END_MISSION(); } + + +#endif /* USE_MISSION_COMMANDS_IN_NPS */ From a6022b96aa1ebfa17fa304c2f129a845536f16be Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 12 Nov 2014 08:02:07 +0100 Subject: [PATCH 06/86] Fix bug in datalink --- sw/airborne/subsystems/datalink/pprz_transport.c | 2 +- sw/airborne/subsystems/datalink/pprzlog_transport.c | 2 +- sw/airborne/subsystems/datalink/xbee.c | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index 0cc85af9f34..ba4808f3881 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -89,7 +89,7 @@ static void end_message(struct pprz_transport *trans, struct link_device *dev) { dev->transmit(dev->periph, trans->ck_a_tx); dev->transmit(dev->periph, trans->ck_b_tx); - dev->send_message(dev); + dev->send_message(dev->periph); } static void overrun(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index 23f42b2e7aa..47952e920de 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -86,7 +86,7 @@ static void start_message(struct pprzlog_transport *trans, struct link_device *d static void end_message(struct pprzlog_transport *trans, struct link_device *dev) { dev->transmit(dev->periph, trans->ck); - dev->send_message(dev); + dev->send_message(dev->periph); } static void overrun(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index e26fe7c1b52..75c8abe48b4 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -95,7 +95,7 @@ static void end_message(struct xbee_transport *trans, struct link_device *dev) { trans->cs_tx = 0xff - trans->cs_tx; dev->transmit(dev->periph, trans->cs_tx); - dev->send_message(dev); + dev->send_message(dev->periph); } static void overrun(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) From 6246b9172187c95acfff3fdcf396e37838fb512e Mon Sep 17 00:00:00 2001 From: agressiva Date: Tue, 11 Nov 2014 16:02:58 -0200 Subject: [PATCH 07/86] [modules] add digital_cam_shoot_rc.xml module to use radio channel as remote camera trigger closes #937 --- conf/modules/digital_cam_shoot_rc.xml | 26 +++++++++ sw/airborne/modules/digital_cam/dc_shoot_rc.c | 55 +++++++++++++++++++ sw/airborne/modules/digital_cam/dc_shoot_rc.h | 35 ++++++++++++ 3 files changed, 116 insertions(+) create mode 100644 conf/modules/digital_cam_shoot_rc.xml create mode 100644 sw/airborne/modules/digital_cam/dc_shoot_rc.c create mode 100644 sw/airborne/modules/digital_cam/dc_shoot_rc.h diff --git a/conf/modules/digital_cam_shoot_rc.xml b/conf/modules/digital_cam_shoot_rc.xml new file mode 100644 index 00000000000..650e0824b3a --- /dev/null +++ b/conf/modules/digital_cam_shoot_rc.xml @@ -0,0 +1,26 @@ + + + + + + Digital camera control using radio channel. + With this module you can take pictures using a radio channel. + When you put the channel to max the picture are taken. + If the channel goes high for more than 0.75s a picture is taken every 1sec. + Only usable for fixedwing firmware. + + + + +
+ +
+ + + + + + + + + diff --git a/sw/airborne/modules/digital_cam/dc_shoot_rc.c b/sw/airborne/modules/digital_cam/dc_shoot_rc.c new file mode 100644 index 00000000000..36efb0f643b --- /dev/null +++ b/sw/airborne/modules/digital_cam/dc_shoot_rc.c @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2014 Eduardo Lavratti + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file modules/digital_cam/dc_shoot_rc.c + * Digital Camera remote shoot using radio channel. + * + * Use radio channel to take a picture. + * Only works with fixedwing firmware. + */ + +#include "dc_shoot_rc.h" +#include "inter_mcu.h" +#include "dc.h" + +#ifndef DC_RADIO_SHOOT +#error "You need to define DC_RADIO_SHOT to a RADIO_xxx channel to use this module" +#endif + +#define DC_RADIO_SHOOT_THRESHOLD 3000 + +void dc_shoot_rc_periodic(void) +{ + static uint8_t rd_shoot = 0; + static uint8_t rd_num = 0; + + if ((rd_shoot == 0) && (fbw_state->channels[DC_RADIO_SHOOT] > DC_RADIO_SHOOT_THRESHOLD)) { + dc_send_command(DC_SHOOT); + rd_shoot = 1; + } + if ((rd_shoot == 1) && (rd_num < 4)) { + rd_num = rd_num + 1; + } + else { + rd_num = 0; + rd_shoot = 0; + } +} diff --git a/sw/airborne/modules/digital_cam/dc_shoot_rc.h b/sw/airborne/modules/digital_cam/dc_shoot_rc.h new file mode 100644 index 00000000000..4437da51ffe --- /dev/null +++ b/sw/airborne/modules/digital_cam/dc_shoot_rc.h @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2014 Eduardo Lavratti + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file modules/digital_cam/dc_shoot_rc.h + * Digital Camera remote shoot using radio channel. + * + * Use radio channel to take a picture. + * Only works with fixedwing firmware. + */ + +#ifndef DC_SHOOT_RC_H +#define DC_SHOOT_RC_H + +/** periodic 4Hz function */ +extern void dc_shoot_rc_periodic(void); + +#endif // DC_SHOOT_RC_H From cd388990acc35a8ff976a8ddd7c204b68f5cdcdb Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 4 Nov 2014 22:18:26 +0100 Subject: [PATCH 08/86] [messages] add INFO_MSG and print it to GCS console Uses a separate string_of_chars function for now to convert the char array from ivy (e.g. |i,n,f,o, ,m,s,g|) to normal string to nicely write to console and messages app. Proper solution would be to write char arrays as normal "quoted string" to the ivy bus. --- conf/messages.xml | 12 ++++++++---- sw/ground_segment/cockpit/live.ml | 8 ++++++++ sw/ground_segment/tmtc/messages.ml | 2 +- sw/lib/ocaml/pprz.ml | 12 ++++++++++++ sw/lib/ocaml/pprz.mli | 1 + 5 files changed, 30 insertions(+), 5 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 52dd6a17803..de0cf7ac82d 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1923,10 +1923,14 @@ - - - - + + + + + + + + diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 814027b7ef5..ad54e389d5d 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1382,6 +1382,13 @@ let listen_error = fun a -> log_and_say a "gcs" msg in safe_bind "TELEMETRY_ERROR" get_error +let listen_info_msg = fun a -> + let get_msg = fun a _sender vs -> + let ac = find_ac _sender in + let msg_array = Pprz.assoc "msg" vs in + log_and_say a ac.ac_name (Pprz.string_of_chars msg_array) in + tele_bind "INFO_MSG" (get_msg a) + let listen_tcas = fun a -> let get_alarm_tcas = fun a txt _sender vs -> let ac = find_ac _sender in @@ -1419,6 +1426,7 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt listen_telemetry_status (); listen_alert my_alert; listen_error my_alert; + listen_info_msg my_alert; listen_tcas my_alert; listen_dcshot geomap; diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index e9cedcda209..4010e83cd66 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -79,7 +79,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m -> with _ -> match format_ with | Some f -> alt_value (Pprz.formatted_string_of_value f x) - | _ -> alt_value (Pprz.string_of_value x) + | _ -> alt_value (Pprz.string_of_chars x) and display_value = fun () -> if notebook#page_num v#coerce = notebook#current_page then if l#label <> !value then l#set_text !value in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 8191b9889e2..31d07c84f4f 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -164,6 +164,18 @@ let rec string_of_value = function | String s -> s | Array a -> "|"^(String.concat separator (Array.to_list (Array.map string_of_value a)))^"|" +let rec string_of_chars = function + Int x -> string_of_int x + | Float x -> string_of_float x + | Int32 x -> Int32.to_string x + | Int64 x -> Int64.to_string x + | Char c -> String.make 1 c + | String s -> s + | Array a -> let vl = Array.to_list (Array.map string_of_chars a) in + match a.(0) with + Char x -> String.concat "" vl + | _ -> "|"^(String.concat separator vl)^"|" + let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index 6eb4657db9b..c57b75a374d 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -63,6 +63,7 @@ val is_fixed_array_type : string -> bool val size_of_field : field -> int val string_of_value : value -> string +val string_of_chars : value -> string val formatted_string_of_value : 'a -> value -> string val int_of_value : value -> int (* May raise Invalid_argument *) type type_descr = { From 0bcb25ecf7b68e54a5e19a8119a6d2e2b7595356 Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 12 Nov 2014 13:30:36 +0100 Subject: [PATCH 09/86] [JTAG][BMP] latest blackmagic firmware easy to update from within paparazzi: closes #871 --- sw/ext/blackmagic/Makefile | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 sw/ext/blackmagic/Makefile diff --git a/sw/ext/blackmagic/Makefile b/sw/ext/blackmagic/Makefile new file mode 100644 index 00000000000..1596dc4d826 --- /dev/null +++ b/sw/ext/blackmagic/Makefile @@ -0,0 +1,16 @@ +# Hey Emacs, this is a -*- makefile -*- + +BLACKSPHERE = http://blacksphere.co.nz/builds +BLACKMAGIC = blackmagic-20141017.bin + +Q=@ + +all: $(BLACKMAGIC) + $(Q)../../tools/stm32loader/stm32loader.py -p /dev/ttyACM0 ./$(BLACKMAGIC) + +$(BLACKMAGIC): + @echo "Downloading latest BMP firmware" + wget -O ./$(BLACKMAGIC) $(BLACKSPHERE)/$(BLACKMAGIC) + +clean: + $(Q) rm -rf ./$(BLACKMAGIC) From 408b2c8bd10e83e9ddd677d24782c4a1aa72f1aa Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 7 Nov 2014 22:47:51 +0100 Subject: [PATCH 10/86] [ocaml][messages] use quotes for char arrays - char[] are now treated as 'real' strings - no more delimiters for other arrays (where space characters should not appear) --- .../subsystems/datalink/ivy_transport.c | 21 ++++++++---- sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/tmtc/messages.ml | 2 +- sw/ground_segment/tmtc/server.ml | 4 +-- sw/lib/ocaml/pprz.ml | 32 ++++++++----------- sw/lib/ocaml/pprz.mli | 1 - 6 files changed, 32 insertions(+), 30 deletions(-) diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index d8188d5eedb..2e76554f7d3 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -41,9 +41,9 @@ static void put_bytes(struct ivy_transport *trans, struct link_device *dev __att { const uint8_t *b = (const uint8_t *) bytes; - // Start delimiter for arrays - if (format == DL_FORMAT_ARRAY) { - trans->ivy_p += sprintf(trans->ivy_p, "|"); + // Start delimiter "quote" for char arrays (strings) + if (format == DL_FORMAT_ARRAY && type == DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, "\""); } int i = 0; @@ -109,17 +109,24 @@ static void put_bytes(struct ivy_transport *trans, struct link_device *dev __att i++; break; } - // Coma delimiter for array, space otherwise + // Coma delimiter for array, no delimiter for char array (string), space otherwise if (format == DL_FORMAT_ARRAY) { - trans->ivy_p += sprintf(trans->ivy_p, ","); + if (type != DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, ","); + } } else { trans->ivy_p += sprintf(trans->ivy_p, " "); } } - // End delimiter for arrays + // space end delimiter for arrays, additionally un-quote char arrays (strings) if (format == DL_FORMAT_ARRAY) { - trans->ivy_p += sprintf(trans->ivy_p, "| "); + if (type == DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, "\" "); + } + else { + trans->ivy_p += sprintf(trans->ivy_p, " "); + } } } diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index ad54e389d5d..df3390a42cf 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1386,7 +1386,7 @@ let listen_info_msg = fun a -> let get_msg = fun a _sender vs -> let ac = find_ac _sender in let msg_array = Pprz.assoc "msg" vs in - log_and_say a ac.ac_name (Pprz.string_of_chars msg_array) in + log_and_say a ac.ac_name (Pprz.string_of_value msg_array) in tele_bind "INFO_MSG" (get_msg a) let listen_tcas = fun a -> diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index 4010e83cd66..e9cedcda209 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -79,7 +79,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m -> with _ -> match format_ with | Some f -> alt_value (Pprz.formatted_string_of_value f x) - | _ -> alt_value (Pprz.string_of_chars x) + | _ -> alt_value (Pprz.string_of_value x) and display_value = fun () -> if notebook#page_num v#coerce = notebook#current_page then if l#label <> !value then l#set_text !value in diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index b457da243c5..1ecee825fca 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -201,7 +201,7 @@ let send_dl_values = fun a -> for i = 0 to a.nb_dl_setting_values - 1 do csv := sprintf "%s%f," !csv a.dl_setting_values.(i) done; - let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String ("|"^ !csv ^"|")] in + let vs = ["ac_id", Pprz.String a.id; "values", Pprz.String !csv] in Ground_Pprz.message_send my_id "DL_VALUES" vs let send_svsinfo = fun a -> @@ -223,7 +223,7 @@ let send_svsinfo = fun a -> concat azim a.svinfo.(i).azim; concat age a.svinfo.(i).age done; - let f = fun s r -> (s, Pprz.String ("|"^ !r ^"|")) in + let f = fun s r -> (s, Pprz.String !r) in let vs = ["ac_id", Pprz.String a.id; "pacc", Pprz.Int a.gps_Pacc; f "svid" svid; f "flags" flags; f "qi" qi; f "msg_age" age; diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 31d07c84f4f..de41089b773 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -146,7 +146,7 @@ let rec value = fun t v -> | Scalar "uint32" -> Int64 (Int64.of_string v) | Scalar ("uint64" | "int64") -> Int64 (Int64.of_string v) | Scalar ("float" | "double") -> Float (float_of_string v) - | Scalar "string" -> String v + | ArrayType "char" | FixedArrayType ("char", _) | Scalar "string" -> String v | Scalar "char" -> Char v.[0] | ArrayType t' -> Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) @@ -162,19 +162,11 @@ let rec string_of_value = function | Int64 x -> Int64.to_string x | Char c -> String.make 1 c | String s -> s - | Array a -> "|"^(String.concat separator (Array.to_list (Array.map string_of_value a)))^"|" - -let rec string_of_chars = function - Int x -> string_of_int x - | Float x -> string_of_float x - | Int32 x -> Int32.to_string x - | Int64 x -> Int64.to_string x - | Char c -> String.make 1 c - | String s -> s - | Array a -> let vl = Array.to_list (Array.map string_of_chars a) in - match a.(0) with - Char x -> String.concat "" vl - | _ -> "|"^(String.concat separator vl)^"|" + | Array a -> + let l = (Array.to_list (Array.map string_of_value a)) in + match a.(0) with + | Char _ -> "\""^(String.concat "" l)^"\"" + | _ -> String.concat separator l let magic = fun x -> (Obj.magic x:('a,'b,'c) Pervasives.format) @@ -194,7 +186,11 @@ let rec formatted_string_of_value = fun format v -> | Int64 x -> sprintf (magic format) x | Char x -> sprintf (magic format) x | String x -> sprintf (magic format) x - | Array a -> "|"^(String.concat separator (Array.to_list (Array.map (formatted_string_of_value format) a)))^"|" + | Array a -> + let l = (Array.to_list (Array.map (formatted_string_of_value format) a)) in + match a.(0) with + | Char _ -> "\""^(String.concat "" l)^"\"" + | _ -> String.concat separator l let sizeof = fun f -> @@ -693,15 +689,15 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let space = Str.regexp "[ \t]+" - let array_sep = Str.regexp "|" + let array_sep = Str.regexp "\"" let values_of_string = fun s -> (* split arguments and arrays *) let array_split = Str.full_split array_sep s in let rec loop = fun fields -> match fields with | [] -> [] - | (Str.Delim "|")::((Str.Text l)::[Str.Delim "|"]) -> [l] - | (Str.Delim "|")::((Str.Text l)::((Str.Delim "|")::xs)) -> [l] @ (loop xs) + | (Str.Delim "\"")::((Str.Text l)::[Str.Delim "\""]) -> [l] + | (Str.Delim "\"")::((Str.Text l)::((Str.Delim "\"")::xs)) -> [l] @ (loop xs) | [Str.Text x] -> Str.split space x | (Str.Text x)::xs -> (Str.split space x) @ (loop xs) | (Str.Delim _)::_ -> failwith "Pprz.values_of_string: incorrect array delimiter" diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index c57b75a374d..6eb4657db9b 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -63,7 +63,6 @@ val is_fixed_array_type : string -> bool val size_of_field : field -> int val string_of_value : value -> string -val string_of_chars : value -> string val formatted_string_of_value : 'a -> value -> string val int_of_value : value -> int (* May raise Invalid_argument *) type type_descr = { From 05e19229d61d6e9d365064ed13bfc62dbd74aac5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 12 Nov 2014 14:25:23 +0100 Subject: [PATCH 11/86] [ocaml] backward compatibility on old array's delimiters --- sw/lib/ocaml/pprz.ml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index de41089b773..85542939d7c 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -689,15 +689,15 @@ module MessagesOfXml(Class:CLASS_Xml) = struct let space = Str.regexp "[ \t]+" - let array_sep = Str.regexp "\"" + let array_sep = Str.regexp "[\"|]" (* also search for old separator '|' for backward compatibility *) let values_of_string = fun s -> (* split arguments and arrays *) let array_split = Str.full_split array_sep s in let rec loop = fun fields -> match fields with | [] -> [] - | (Str.Delim "\"")::((Str.Text l)::[Str.Delim "\""]) -> [l] - | (Str.Delim "\"")::((Str.Text l)::((Str.Delim "\"")::xs)) -> [l] @ (loop xs) + | (Str.Delim "\"")::((Str.Text l)::[Str.Delim "\""]) | (Str.Delim "|")::((Str.Text l)::[Str.Delim "|"]) -> [l] + | (Str.Delim "\"")::((Str.Text l)::((Str.Delim "\"")::xs)) | (Str.Delim "|")::((Str.Text l)::((Str.Delim "|")::xs)) -> [l] @ (loop xs) | [Str.Text x] -> Str.split space x | (Str.Text x)::xs -> (Str.split space x) @ (loop xs) | (Str.Delim _)::_ -> failwith "Pprz.values_of_string: incorrect array delimiter" From 88e133bc4c76551cd5ffc409ccaea3b7026d1053 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 12 Nov 2014 16:30:00 +0100 Subject: [PATCH 12/86] [python] update messages tool to deal with whitespaces in arrays --- sw/lib/python/messages_tool.py | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/sw/lib/python/messages_tool.py b/sw/lib/python/messages_tool.py index c4b5950bb60..7eaa0670245 100644 --- a/sw/lib/python/messages_tool.py +++ b/sw/lib/python/messages_tool.py @@ -5,6 +5,7 @@ import logging import time import os +import re class Message: def __init__(self, class_name, name): @@ -49,7 +50,21 @@ def InitIvy(self, initIvy): self.ivy_id = IvyBindMsg(self.OnIvyMsg, "(.*)") def OnIvyMsg(self, agent, *larg): - data = larg[0].split(' ') + """ Split ivy message up into the separate parts + Basically parts/args in string are separated by space, but char array can also contain a space: + |f,o,o, ,b,a,r| in old format or "foo bar" in new format + """ + # first split on array delimiters + l = re.split('([|\"][^|]*[|\"])', larg[0]) + # strip spaces and filter out emtpy strings + l = [str.strip(s) for s in l if str.strip(s) is not ''] + data = [] + for s in l: + # split non-array strings further up + if '|' not in s and '"' not in s: + data += s.split(' ') + else: + data.append(s) try: ac_id = int(data[0]) name = data[1] From f9323b9c4c6e7039bfd1b446b40d09d04fac6f2b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 12 Nov 2014 23:23:16 +0100 Subject: [PATCH 13/86] [filters] add missing include to low_pass_filter.h --- sw/airborne/filters/low_pass_filter.h | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/filters/low_pass_filter.h b/sw/airborne/filters/low_pass_filter.h index 4d6d6a2bb3d..92f0f476a09 100644 --- a/sw/airborne/filters/low_pass_filter.h +++ b/sw/airborne/filters/low_pass_filter.h @@ -28,6 +28,7 @@ #define LOW_PASS_FILTER_H #include "std.h" +#include "math/pprz_algebra_int.h" #define INT32_FILT_FRAC 8 From 99120759f21935a96d0573c6fe0745aa8a996cc3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 12 Nov 2014 23:30:23 +0100 Subject: [PATCH 14/86] [modules] fix some more modules after conversion to telemetry functions --- sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c | 4 ++-- sw/airborne/modules/sensors/temp_adc.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 1bf4ff0c418..22a8ba7062a 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c @@ -137,7 +137,7 @@ static struct i2c_transaction ms45xx_trans; static Butterworth2LowPass ms45xx_filter; -static void ms45xx_downlink(void) +static void ms45xx_downlink(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_AIRSPEED_MS45XX(trans, dev, AC_ID, &ms45xx.diff_pressure, @@ -213,7 +213,7 @@ void ms45xx_i2c_event(void) stateSetAirspeed_f(&ms45xx.airspeed); #endif if (ms45xx.sync_send) { - ms45xx_downlink(); + ms45xx_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } } diff --git a/sw/airborne/modules/sensors/temp_adc.c b/sw/airborne/modules/sensors/temp_adc.c index 2a1c04f729b..1a610d91796 100644 --- a/sw/airborne/modules/sensors/temp_adc.c +++ b/sw/airborne/modules/sensors/temp_adc.c @@ -98,7 +98,7 @@ float calc_lm35(int16_t raw_temp) return ((float)raw_temp * (3300.0f / 1024.0f) / 10.0f); } -static void temp_adc_downlink(void) +static void temp_adc_downlink(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_TEMP_ADC(trans, dev, AC_ID, &temp_c1, &temp_c2, &temp_c3); } @@ -157,7 +157,7 @@ void temp_adc_periodic(void) #endif if (temp_adc_sync_send) { - temp_adc_downlink(); + temp_adc_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device); } } From 377ecfa0075e8a57df67eea79a5d36d2b2a45558 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 12 Nov 2014 23:31:36 +0100 Subject: [PATCH 15/86] [conf] add aispeed_ms45xx_i2c module to Quad_Navstik for testing --- conf/airframes/examples/quadrotor_navstik.xml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/conf/airframes/examples/quadrotor_navstik.xml b/conf/airframes/examples/quadrotor_navstik.xml index 1c39eb92c10..3101c96459c 100644 --- a/conf/airframes/examples/quadrotor_navstik.xml +++ b/conf/airframes/examples/quadrotor_navstik.xml @@ -44,7 +44,10 @@ - + + + + From d7bbfdf656ab7b1d44ffe83130f79084e8d09bde Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 15:34:25 +0100 Subject: [PATCH 16/86] [build] quiet output on clean_ac --- Makefile.ac | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Makefile.ac b/Makefile.ac index 7e8ec2960aa..d226572f17a 100644 --- a/Makefile.ac +++ b/Makefile.ac @@ -214,7 +214,8 @@ fbw : fbw.compile ap: ap.compile clean_ac : - $(Q)if (expr "$(AIRCRAFT)"); then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi - rm -fr $(AIRCRAFT_BUILD_DIR) + $(Q)if (expr "$(AIRCRAFT)") > /dev/null; then : ; else echo "AIRCRAFT undefined: type 'make AIRCRAFT=AircraftName ...'"; exit 1; fi + @echo "CLEANING $(AIRCRAFT)" + $(Q)rm -fr $(AIRCRAFT_BUILD_DIR) .PHONY: init all_ac_h radio_ac_h flight_plan_ac_h makefile_ac clean_ac print_version From f62c56f4e93df701c0b3031e330ab2421283dc32 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 16:58:04 +0100 Subject: [PATCH 17/86] [tests] option to show warnings even if successfully compiled Examples on how to test compile all aircrafts/targets in your current conf.xml: with parallel compilation and showing full output during compilation $ J=AUTO prove tests/examples -v only showing full compile output if there has been an error, if there were warnings only print those $ SHOW_WARNINGS_ONLY=1 prove test/examples with parallel compilation and treating all warnings as errors: $ J=AUTO USER_CFLAGS=-Werror prove tests/examples --- tests/examples/01_compile_all_aircrafts.t | 119 ++++++++++++++-------- 1 file changed, 75 insertions(+), 44 deletions(-) diff --git a/tests/examples/01_compile_all_aircrafts.t b/tests/examples/01_compile_all_aircrafts.t index 9c14937f010..d535711af1d 100644 --- a/tests/examples/01_compile_all_aircrafts.t +++ b/tests/examples/01_compile_all_aircrafts.t @@ -10,11 +10,17 @@ # # optional environment variables: # TEST_VERBOSE : set to 1 to print the compile output even if there was no error +# SHOW_WARNINGS : set to 1 to print the complete compile output if there were warnings +# SHOW_WARNINGS_ONLY : set to 1 to print only the warnings # # environment variables passed on to make: # J=AUTO : detect number of CPUs to set jobs for parallel compilation # -# Example on how to test compile all aircrafts/targets in your current conf.xml +# Examples on how to test compile all aircrafts/targets in your current conf.xml: +# with parallel compilation and showing full output during compilation +# J=AUTO prove tests/examples -v +# only showing full compile output if there has been an error, if there were warnings only print those +# SHOW_WARNINGS_ONLY=1 prove test/examples # with parallel compilation and treating all warnings as errors: # J=AUTO USER_CFLAGS=-Werror prove tests/examples # @@ -22,9 +28,10 @@ use Test::More; use lib "$ENV{'PAPARAZZI_SRC'}/tests/lib"; use XML::Simple; -use Program; use Data::Dumper; use Config; +use IPC::Run qw( run ); +use Cwd; $|++; my $xmlSimple = XML::Simple->new(ForceArray => 1); @@ -42,13 +49,32 @@ foreach my $aircraft (sort keys%{$conf->{'aircraft'}}) { #warn "AIRCRAFT: [$aircraft] TARGET: [$target]\n"; my $make_options = "AIRCRAFT=$aircraft clean_ac $target.compile"; - my ($exit_status, $output) = run_program( + my ($exit_status, $warnings, $output) = run_program( "Attempting to build the firmware $target for the aircraft $aircraft.", $ENV{'PAPARAZZI_SRC'}, "make $make_options", - $ENV{'TEST_VERBOSE'},1); - # print output if it failed and we didn't already print it in verbose mode - warn "$output\n" if $exit_status && !$ENV{'TEST_VERBOSE'}; + $ENV{'TEST_VERBOSE'}); + + # if we didn't already print output in verbose mode, + # print if it failed + if ($exit_status && !$ENV{'TEST_VERBOSE'}) { + warn "$output\n"; + } + # if successful, still print warnings if requested + elsif ($warnings && ($ENV{'SHOW_WARNINGS'} || $ENV{'SHOW_WARNINGS_ONLY'})) { + if (!$ENV{'TEST_VERBOSE'}) { + warn "\nWarning: AIRCRAFT=$aircraft target=$target compiled sucessfully but had warnings:\n"; + if ($ENV{'SHOW_WARNINGS_ONLY'}) { + warn "$warnings\n"; + } + else { + warn "$output\n"; + } + } + if (!$ENV{'SHOW_WARNINGS_ONLY'}) { + warn "\nAIRCRAFT=$aircraft target=$target compiled sucessfully but had warnings.\n\n"; + } + } ok($exit_status == 0, "Compile aircraft: $aircraft, target: $target"); } } @@ -58,46 +84,51 @@ done_testing(); ################################################################################ # functions used by this test script. + sub run_program { - my $message = shift; - my $dir = shift; - my $command = shift; - my $verbose = shift; - my $dont_fail_on_error = shift; - - warn "\n$message\n" if $verbose; - if (defined $dir) - { - $command = "cd $dir;" . $command; - } - my $prog = new Program("bash"); - #$prog->redirect('none'); - my $fh = $prog->open("-c \"$command\""); - warn "Running command: \"". $prog->last_command() ."\"\n" if $verbose; - $fh->autoflush(1); - my @output; - while (<$fh>) - { - warn $_ if $verbose; - chomp $_; - push @output, $_; - } - $fh->close; - my $exit_status = $?/256; - unless ($exit_status == 0) - { - my $err_msg = "\nError: The command \"". $prog->last_command() ."\" failed to complete successfully. Exit status: $exit_status\n"; - if ($dont_fail_on_error) - { - warn $err_msg; - } - else - { - die $err_msg; - } + my $message = shift; + my $dir = shift; + my $command = shift; + my $verbose = shift; + + warn "\n$message\n" if $verbose; + warn "Running command: \"". $command ."\"\n" if $verbose; + + # change into specified dir and remember current working dir + my $working_dir = cwd; + if (defined $dir) { + chdir $dir; + } + + my $warnings = ''; + my $stderr_and_out = ''; + + my $stdout_handler = sub { + print @_ if $verbose; + $stderr_and_out .= $_[0]; + }; + my $stderr_handler = sub { + print @_ if $verbose; + # check if output on stderr contains warnings, but ignoring "Warning: low altitude" + if ($_[0] =~ /warning/i && $_[0] !~ /Warning: low altitude/) { + $warnings .= $_[0]."\n"; + #warn "\ndetected warning in $_[0]\n"; } - my $output_string = join "\n", @output; - return ($exit_status, $output_string); + $stderr_and_out .= $_[0]; + }; + my $dummy_in; + my $run = run([split ' ', $command], \$dummy_in, $stdout_handler, $stderr_handler); + my $exit_status = $?/256; + + # change back to original dir + chdir $working_dir; + + unless ($exit_status == 0) + { + warn "\nError: The command \"". $command ."\" failed to complete successfully. Exit status: $exit_status\n"; + } + + return ($exit_status, $warnings, $stderr_and_out); } From 0c4dad2c984aeb43e75b335d908df6db16c896bc Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 17:25:49 +0100 Subject: [PATCH 18/86] update travis config to show warnings --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 011ebf7e6a2..aea74e853a4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,7 @@ install: before_script: cd conf && ln -s conf_tests.xml conf.xml && cd .. script: - make - - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD J=AUTO prove tests/examples/ + - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD SHOW_WARNINGS_ONLY=1 J=AUTO prove tests/examples/ notifications: webhooks: From 357b2df9fa73027d40fca02ce6478776d47ccf20 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 17:50:28 +0100 Subject: [PATCH 19/86] install libipc-run-perl in travis setup --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index aea74e853a4..741d22c15da 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,7 +7,7 @@ before_install: - sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded -y - sudo apt-get update -q install: - - sudo apt-get install paparazzi-dev paparazzi-jsbsim gcc-arm-none-eabi + - sudo apt-get install paparazzi-dev paparazzi-jsbsim gcc-arm-none-eabi libipc-run-perl before_script: cd conf && ln -s conf_tests.xml conf.xml && cd .. script: - make From 44b929f8d88f80b62d56a3e874abb34cdcdcdbf6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 17:24:29 +0100 Subject: [PATCH 20/86] [simulator] ignore stupid unused-parameter warnings in JSBSim --- sw/simulator/sim_ac_flightgear.c | 6 ++++++ sw/simulator/sim_ac_jsbsim.c | 5 +++++ sw/simulator/sim_ac_jsbsim.h | 4 ++++ 3 files changed, 15 insertions(+) diff --git a/sw/simulator/sim_ac_flightgear.c b/sw/simulator/sim_ac_flightgear.c index 0817ce63eb9..65d0dbf199c 100644 --- a/sw/simulator/sim_ac_flightgear.c +++ b/sw/simulator/sim_ac_flightgear.c @@ -28,7 +28,13 @@ #include #include #include "std.h" + +// ignore stupid warnings in JSBSim +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" #include +#pragma GCC diagnostic pop + #include "flight_gear.h" #include "sim_ac_flightgear.h" diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index a3494d9c4bc..17bef88227e 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -29,9 +29,14 @@ #include +// ignore stupid warnings in JSBSim +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" #include //#include #include +#pragma GCC diagnostic pop + #include "sim_ac_flightgear.h" using namespace std; diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index 9b9b20ebeef..85e893712ed 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -23,9 +23,13 @@ #ifndef SIM_AC_JSBSIM_H #define SIM_AC_JSBSIM_H +// ignore stupid warnings in JSBSim +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" #include #include #include +#pragma GCC diagnostic pop #include "std.h" #include "generated/airframe.h" From 3836df01d56fa669699282628daa40de0b802c7c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Nov 2014 12:53:19 +0100 Subject: [PATCH 21/86] [nps] add missing include --- sw/simulator/nps/nps_ivy_fixedwing.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c index a7bd73cbba5..0172dbca2f9 100644 --- a/sw/simulator/nps/nps_ivy_fixedwing.c +++ b/sw/simulator/nps/nps_ivy_fixedwing.c @@ -1,6 +1,7 @@ #include "nps_ivy.h" #include +#include #include #include "generated/airframe.h" From a0ecc7859802b02d086e7973df19ce74e6041d65 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Nov 2014 12:54:01 +0100 Subject: [PATCH 22/86] [airborne] get rid of some const-qual warnings --- sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c | 2 +- sw/airborne/subsystems/ins/ins_gps_passthrough.c | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c index 66d884c5ecd..a2a5be25a6a 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c @@ -391,7 +391,7 @@ void ahrs_update_mag_2d(float dt) { // normalize measured ltp in 2D (x,y) float_vect2_normalize(&measured_ltp_2d); - const struct FloatVect3 residual_ltp = + struct FloatVect3 residual_ltp = { 0, 0, measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 77d5b82c5e1..5dcf7b7f34c 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -68,7 +68,8 @@ static void send_ins(struct transport_tx *trans, struct link_device *dev) { static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { static const float fake_baro_z = 0.0; pprz_msg_send_INS_Z(trans, dev, AC_ID, - &fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); + (float*)&fake_baro_z, &ins_impl.ltp_pos.z, + &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); } static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { @@ -77,7 +78,7 @@ static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_INS_REF(trans, dev, AC_ID, &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, - &ins_impl.ltp_def.hmsl, &fake_qfe); + &ins_impl.ltp_def.hmsl, (float*)&fake_qfe); } } #endif From de200404abec6a0f869b804be92c35da2f318fd5 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 14 Nov 2014 15:04:20 +0100 Subject: [PATCH 23/86] [settings] fix multiple settings in modules close #901 --- sw/lib/ocaml/env.ml | 46 ++++++++++++++++++++--------- sw/lib/ocaml/env.mli | 3 ++ sw/lib/ocaml/gen_common.ml | 4 +-- sw/lib/ocaml/gen_common.mli | 2 -- sw/supervision/pc_aircraft.ml | 8 ++--- sw/tools/generators/gen_aircraft.ml | 2 +- sw/tools/generators/gen_settings.ml | 2 +- 7 files changed, 42 insertions(+), 25 deletions(-) diff --git a/sw/lib/ocaml/env.ml b/sw/lib/ocaml/env.ml index 3d303115a86..5c3e7b136b3 100644 --- a/sw/lib/ocaml/env.ml +++ b/sw/lib/ocaml/env.ml @@ -56,6 +56,8 @@ let gcs_icons_path = paparazzi_home // "data" // "pictures" // "gcs_icons" let dump_fp = paparazzi_src // "sw" // "tools" // "generators" // "gen_flight_plan.out -dump" +let default_module_targets = "ap|sim|nps" + let filter_absolute_path = fun path -> Str.replace_first (Str.regexp (paparazzi_home // "conf/")) "" path @@ -67,26 +69,42 @@ let filter_settings = fun settings -> String.concat " " sl (* filter on modules based on target *) -let filter_modules_target = fun module_xml -> +let filter_modules_target = fun module_file -> + (* get TARGET env *) let target = try Sys.getenv "TARGET" with _ -> "" in + (* look for a specific name after settings file (in case of modules) *) + let split = Str.split (Str.regexp "~") module_file in + let xml_file, name = match split with + | [f; n] -> f, n + | _ -> module_file, "" + in + let module_xml = Xml.parse_file xml_file in if Xml.tag module_xml = "module" - then begin - (* test if the module is loaded or not *) - if List.exists (fun n -> - let t = ExtXml.attrib_or_default n "target" "" in - Str.string_match (Str.regexp (".*"^target^".*")) t 0 - ) (Xml.children module_xml) - then Xml.Element ("settings", [], List.filter (fun t -> Xml.tag t = "settings") (Xml.children module_xml)) - else Xml.Element ("",[],[]) - end - else module_xml + then + begin + (* test if the module is loaded or not + * and if a specific sub-settings is selected *) + if List.exists (fun n -> + let local_target = ExtXml.attrib_or_default n "target" default_module_targets + and tag = Xml.tag n in + if tag = "makefile" then + Str.string_match (Str.regexp (".*"^target^".*")) local_target 0 + else false + ) (Xml.children module_xml) + then Xml.Element ("settings", [], + List.filter (fun t -> + Xml.tag t = "settings" && ExtXml.attrib_or_default t "name" "" = name) + (Xml.children module_xml)) + else Xml.Element ("",[],[]) + end + else module_xml let expand_ac_xml = fun ?(raise_exception = true) ac_conf -> let prefix = fun s -> sprintf "%s/conf/%s" paparazzi_home s in - let parse_file = fun ?(parse_filter=(fun x -> x)) a file -> + let parse_file = fun ?(parse_filter=(fun x -> ExtXml.parse_file x)) a file -> try - parse_filter (ExtXml.parse_file file) + parse_filter file with Failure msg -> if raise_exception then @@ -96,7 +114,7 @@ let expand_ac_xml = fun ?(raise_exception = true) ac_conf -> make_element "parse error" ["file",a; "msg", msg] [] end in - let parse = fun ?(pre_filter=(fun x -> x)) ?(parse_filter=(fun x -> x)) a -> + let parse = fun ?(pre_filter=(fun x -> x)) ?(parse_filter=(fun x -> ExtXml.parse_file x)) a -> List.map (fun filename -> parse_file ~parse_filter a (prefix filename)) (Str.split space_regexp (pre_filter (ExtXml.attrib ac_conf a))) in diff --git a/sw/lib/ocaml/env.mli b/sw/lib/ocaml/env.mli index 54d93030880..8028ec1cdaf 100644 --- a/sw/lib/ocaml/env.mli +++ b/sw/lib/ocaml/env.mli @@ -48,6 +48,9 @@ val gconf_file : string val gcs_icons_path : string +(* Default targets for modules *) +val default_module_targets : string + val filter_absolute_path : string -> string (** remove absolute path paparazzi_home/conf if it exists * returns a relative path *) diff --git a/sw/lib/ocaml/gen_common.ml b/sw/lib/ocaml/gen_common.ml index ee02da8498d..008500ef41f 100644 --- a/sw/lib/ocaml/gen_common.ml +++ b/sw/lib/ocaml/gen_common.ml @@ -32,8 +32,6 @@ let paparazzi_conf = Env.paparazzi_home // "conf" let modules_dir = paparazzi_conf // "modules" let autopilot_dir = paparazzi_conf // "autopilot" -let default_module_targets = "ap|sim|nps" - (** remove all duplicated elements of a list *) let singletonize = fun l -> let rec loop = fun l -> @@ -120,7 +118,7 @@ let rec get_modules_of_airframe = fun xml -> let get_targets_of_module = fun conf -> let targets = List.map (fun x -> match String.lowercase (Xml.tag x) with - "makefile" -> targets_of_field x default_module_targets + "makefile" -> targets_of_field x Env.default_module_targets | _ -> [] ) (Xml.children conf.xml) in let targets = (List.flatten targets) @ conf.extra_targets in diff --git a/sw/lib/ocaml/gen_common.mli b/sw/lib/ocaml/gen_common.mli index a8071275628..750adb046b8 100644 --- a/sw/lib/ocaml/gen_common.mli +++ b/sw/lib/ocaml/gen_common.mli @@ -33,8 +33,6 @@ type module_conf = { xml : Xml.xml; file : string; vpath : string option; param (* Modules directory *) val modules_dir : string -(* Default targets for modules *) -val default_module_targets : string (** remove all duplicated elements of a list *) val singletonize : 'a list -> 'a list diff --git a/sw/supervision/pc_aircraft.ml b/sw/supervision/pc_aircraft.ml index 2551bb16b53..e027d8e8d42 100644 --- a/sw/supervision/pc_aircraft.ml +++ b/sw/supervision/pc_aircraft.ml @@ -174,7 +174,7 @@ let get_settings_modules = fun ac_xml settings_modules -> let file_list = List.map (fun s -> "settings/"^(Xml.attrib s "name")) settings_file_list in (* include module file in the list only if it has a 'settings' node *) let settings_list = List.filter (fun t -> Xml.tag t = "settings") (Xml.children m) in - let module_file = if List.length settings_list > 0 then [Env.filter_absolute_path f] else [] in + (*let module_file = if List.length settings_list > 0 then [Env.filter_absolute_path f] else [] in*) (* include module file with specific name if they exist *) let settings_list = List.fold_left (fun l s -> try @@ -184,10 +184,10 @@ let get_settings_modules = fun ac_xml settings_modules -> then failwith "Paparazzicenter: no white space allowed in modules settings name"; l @ [(Env.filter_absolute_path f)^"~"^name^"~"] with - | Failure x -> prerr_endline x; l - | _ -> l + | Failure x -> prerr_endline x; l @ [Env.filter_absolute_path f] + | _ -> l @ [Env.filter_absolute_path f] ) [] settings_list in - l @ file_list @ module_file @ settings_list + l @ file_list (*@ module_file*) @ settings_list ) [] modules in (* store current state in a hashtable *) let current = Hashtbl.create 7 in diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml index 9d42c484e29..82e0113d6f3 100644 --- a/sw/tools/generators/gen_aircraft.ml +++ b/sw/tools/generators/gen_aircraft.ml @@ -116,7 +116,7 @@ let dump_module_section = fun xml f -> (* add extra targets only if default is used *) let et = try ignore(Xml.attrib l "target"); [] with _ -> m.extra_targets in let targets = Gen_common.singletonize ( - Gen_common.targets_of_field l Gen_common.default_module_targets @ et) in + Gen_common.targets_of_field l Env.default_module_targets @ et) in (* Look for defines, flags, files, ... *) List.iter (fun field -> match String.lowercase (Xml.tag field) with diff --git a/sw/tools/generators/gen_settings.ml b/sw/tools/generators/gen_settings.ml index 6296e05c64b..56afd9a863e 100644 --- a/sw/tools/generators/gen_settings.ml +++ b/sw/tools/generators/gen_settings.ml @@ -291,7 +291,7 @@ let join_xml_files = fun xml_files -> (* test if the module is loaded or not *) if List.exists (fun n -> if Xml.tag n = "makefile" then begin - let t = ExtXml.attrib_or_default n "target" Gen_common.default_module_targets in + let t = ExtXml.attrib_or_default n "target" Env.default_module_targets in Str.string_match (Str.regexp (".*"^target^".*")) t 0 end else false From e670d2b70c16d8cfb1cd2eb6479e0757fd1abaa0 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 14 Nov 2014 15:09:16 +0100 Subject: [PATCH 24/86] [build] remove 'load' target from makefile It is a special command in Make 4.0 --- conf/Makefile.ardrone2 | 2 +- conf/Makefile.geode | 2 +- conf/Makefile.lpc21 | 2 +- conf/Makefile.pentium-m | 2 +- sw/airborne/arch/lpc21/test/Makefile | 6 +++--- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/conf/Makefile.ardrone2 b/conf/Makefile.ardrone2 index ad6e9f254ba..3a8967ef2aa 100644 --- a/conf/Makefile.ardrone2 +++ b/conf/Makefile.ardrone2 @@ -32,7 +32,7 @@ upload_extra: # Program the device and start it. -load upload program: upload_extra $(OBJDIR)/$(TARGET).elf +upload program: upload_extra $(OBJDIR)/$(TARGET).elf $(Q)$(DRONE) --host=$(HOST) insmod $(PAPARAZZI_SRC)/sw/ext/ardrone2_drivers/cdc-acm.ko $(Q)$(DRONE) --host=$(HOST) upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR) $(Q)$(DRONE) --host=$(HOST) status diff --git a/conf/Makefile.geode b/conf/Makefile.geode index ce38405faa8..ffff7d98eab 100644 --- a/conf/Makefile.geode +++ b/conf/Makefile.geode @@ -67,7 +67,7 @@ $(OBJDIR): elf: $(OBJDIR)/$(TARGET).elf # Program the device. -load upload program: $(OBJDIR)/$(TARGET).elf +upload program: $(OBJDIR)/$(TARGET).elf scp $(OBJDIR)/$(TARGET).elf $(USER)@$(HOST):$(TARGET_DIR) # Link: create ELF output file from object files. diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 117cf4607e4..1e1654f52b8 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -192,7 +192,7 @@ sym: $(OBJDIR)/$(TARGET).sym # Program the device. -load upload program: $(OBJDIR)/$(TARGET).hex +upload program: $(OBJDIR)/$(TARGET).hex ifeq ($(FLASH_MODE),IAP) $(SUDO) $(LPC21IAP) $(OBJDIR)/$(TARGET).elf else ifeq ($(FLASH_MODE),JTAG) diff --git a/conf/Makefile.pentium-m b/conf/Makefile.pentium-m index ed9ee530d44..a40f49da0c4 100644 --- a/conf/Makefile.pentium-m +++ b/conf/Makefile.pentium-m @@ -76,7 +76,7 @@ $(OBJDIR): elf: $(OBJDIR)/$(TARGET).elf # Program the device. -load upload program: $(OBJDIR)/$(TARGET).elf +upload program: $(OBJDIR)/$(TARGET).elf ifdef USER ssh $(USER)@$(HOST) "sudo mount -o remount,rw /" scp $(OBJDIR)/$(TARGET).elf $(USER)@$(HOST):$(TARGET_DIR) diff --git a/sw/airborne/arch/lpc21/test/Makefile b/sw/airborne/arch/lpc21/test/Makefile index 374f0684bd7..852607074f2 100644 --- a/sw/airborne/arch/lpc21/test/Makefile +++ b/sw/airborne/arch/lpc21/test/Makefile @@ -15,7 +15,7 @@ # # make clean = Clean out built project files. # -# make load = Download the hex file to the device, using lpc21isp +# make upload = Download the hex file to the device, using lpc21isp # # (TODO: make filename.s = Just compile filename.c into the assembler code only) # @@ -297,7 +297,7 @@ gccversion : # Program the device. -load upload program: $(TARGET).hex +upload program: $(TARGET).hex ifeq ($(FLASH_MODE),IAP) $(LPC21IAP) $(TARGET).elf else @@ -423,4 +423,4 @@ clean_list : # Listing of phony targets. .PHONY : all begin finish end sizebefore sizeafter gccversion \ -build elf hex lss sym clean clean_list load +build elf hex lss sym clean clean_list From df7a06fa8797f4ea16d8cb7a4b028be55955ee47 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 14 Nov 2014 15:17:42 +0100 Subject: [PATCH 25/86] [build] remove unused load from phony list --- conf/Makefile.ardrone2 | 4 ++-- conf/Makefile.lpc21 | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/conf/Makefile.ardrone2 b/conf/Makefile.ardrone2 index 3a8967ef2aa..2f92b83e9b4 100644 --- a/conf/Makefile.ardrone2 +++ b/conf/Makefile.ardrone2 @@ -39,7 +39,7 @@ upload program: upload_extra $(OBJDIR)/$(TARGET).elf # Program the device and start it. -load2 upload2 program2: $(OBJDIR)/$(TARGET).elf +upload2 program2: $(OBJDIR)/$(TARGET).elf # Kill the application -echo "killall -9 $(TARGET).elf" | telnet $(HOST) @@ -82,4 +82,4 @@ ifeq ($(ARDRONE2_REBOOT),1) endif # Listing of phony targets. -.PHONY : upload_extra load upload program load2 upload2 program2 +.PHONY : upload_extra upload program upload2 program2 diff --git a/conf/Makefile.lpc21 b/conf/Makefile.lpc21 index 1e1654f52b8..1315d58f299 100644 --- a/conf/Makefile.lpc21 +++ b/conf/Makefile.lpc21 @@ -267,7 +267,7 @@ $(AOBJARM) : $(OBJDIR)/%.o : $(SRC_ARCH)/%.S # Listing of phony targets. -.PHONY : all build elf hex lss sym load upload program +.PHONY : all build elf hex lss sym upload program # From 9e50655d948c2a126a84e9739d005e991b4cb63b Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 16 Nov 2014 11:35:15 +0100 Subject: [PATCH 26/86] [peripherals] add mpu9250 driver --- sw/airborne/peripherals/mpu9250.c | 118 +++++++++++++++ sw/airborne/peripherals/mpu9250.h | 113 ++++++++++++++ sw/airborne/peripherals/mpu9250_i2c.c | 184 +++++++++++++++++++++++ sw/airborne/peripherals/mpu9250_i2c.h | 85 +++++++++++ sw/airborne/peripherals/mpu9250_regs.h | 199 +++++++++++++++++++++++++ sw/airborne/peripherals/mpu9250_spi.c | 177 ++++++++++++++++++++++ sw/airborne/peripherals/mpu9250_spi.h | 84 +++++++++++ 7 files changed, 960 insertions(+) create mode 100644 sw/airborne/peripherals/mpu9250.c create mode 100644 sw/airborne/peripherals/mpu9250.h create mode 100644 sw/airborne/peripherals/mpu9250_i2c.c create mode 100644 sw/airborne/peripherals/mpu9250_i2c.h create mode 100644 sw/airborne/peripherals/mpu9250_regs.h create mode 100644 sw/airborne/peripherals/mpu9250_spi.c create mode 100644 sw/airborne/peripherals/mpu9250_spi.h diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c new file mode 100644 index 00000000000..ebdb23cf88f --- /dev/null +++ b/sw/airborne/peripherals/mpu9250.c @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2013 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250.c + * + * MPU-9250 driver common functions (I2C and SPI). + * + * Still needs the either the I2C or SPI specific implementation. + */ + +#include "peripherals/mpu9250.h" + +void mpu9250_set_default_config(struct Mpu9250Config *c) +{ + c->clk_sel = MPU9250_DEFAULT_CLK_SEL; + c->smplrt_div = MPU9250_DEFAULT_SMPLRT_DIV; + c->dlpf_gyro_cfg = MPU9250_DEFAULT_DLPF_GYRO_CFG; + c->dlpf_accel_cfg = MPU9250_DEFAULT_DLPF_ACCEL_CFG; + c->gyro_range = MPU9250_DEFAULT_FS_SEL; + c->accel_range = MPU9250_DEFAULT_AFS_SEL; + c->drdy_int_enable = FALSE; + + /* Number of bytes to read starting with MPU9250_REG_INT_STATUS + * By default read only gyro and accel data -> 15 bytes. + * Increase to include slave data. + */ + c->nb_bytes = 15; + c->nb_slaves = 0; + + c->i2c_bypass = FALSE; +} + +void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config) +{ + switch (config->init_status) { + case MPU9250_CONF_RESET: + /* device reset, set register values to defaults */ + mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, (1<<6)); + config->init_status++; + break; + case MPU9250_CONF_USER_RESET: + /* trigger FIFO, I2C_MST and SIG_COND resets */ + mpu_set(mpu, MPU9250_REG_USER_CTRL, ((1 << MPU9250_FIFO_RESET) | + (1 << MPU9250_I2C_MST_RESET) | + (1 << MPU9250_SIG_COND_RESET))); + config->init_status++; + break; + case MPU9250_CONF_PWR: + /* switch to gyroX clock by default */ + mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6))); + config->init_status++; + break; + case MPU9250_CONF_SD: + /* configure sample rate divider */ + mpu_set(mpu, MPU9250_REG_SMPLRT_DIV, config->smplrt_div); + config->init_status++; + break; + case MPU9250_CONF_DLPF_GYRO: + /* configure digital low pass filter for gyroscope */ + mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_gyro_cfg); + config->init_status++; + break; + case MPU9250_CONF_DLPF_ACCEL: + /* configure digital low pass filter fir accelerometer */ + mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_accel_cfg); + config->init_status++; + break; + case MPU9250_CONF_GYRO: + /* configure gyro range */ + mpu_set(mpu, MPU9250_REG_GYRO_CONFIG, (config->gyro_range<<3)); + config->init_status++; + break; + case MPU9250_CONF_ACCEL: + /* configure accelerometer range */ + mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG, (config->accel_range<<3)); + config->init_status++; + break; + case MPU9250_CONF_I2C_SLAVES: + /* if any, set MPU for I2C slaves and configure them*/ + if (config->nb_slaves > 0) { + /* returns TRUE when all slaves are configured */ + if (mpu9250_configure_i2c_slaves(mpu_set, mpu)) + config->init_status++; + } + else + config->init_status++; + break; + case MPU9250_CONF_INT_ENABLE: + /* configure data ready interrupt */ + mpu_set(mpu, MPU9250_REG_INT_ENABLE, (config->drdy_int_enable<<0)); + config->init_status++; + break; + case MPU9250_CONF_DONE: + config->initialized = TRUE; + break; + default: + break; + } +} diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h new file mode 100644 index 00000000000..90d906c1964 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250.h @@ -0,0 +1,113 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250.h + * + * MPU-60X0 driver common interface (I2C and SPI). + */ + +#ifndef MPU9250_H +#define MPU9250_H + +#include "std.h" + +/* Include address and register definition */ +#include "peripherals/mpu9250_regs.h" + +/// Default sample rate divider +#define MPU9250_DEFAULT_SMPLRT_DIV 0 +/// Default gyro full scale range +- 2000°/s +#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_2000 +/// Default accel full scale range +- 16g +#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_16G +/// Default internal sampling (1kHz, 42Hz LP Bandwidth) +#define MPU9250_DEFAULT_DLPF_ACCEL_CFG MPU9250_DLPF_ACCEL_41HZ +/// Default internal sampling (1kHz, 42Hz LP Bandwidth) +#define MPU9250_DEFAULT_DLPF_GYRO_CFG MPU9250_DLPF_GYRO_41HZ +/// Default interrupt config: DATA_RDY_EN +#define MPU9250_DEFAULT_INT_CFG 1 +/// Default clock: PLL with X gyro reference +#define MPU9250_DEFAULT_CLK_SEL 1 + +enum Mpu9250ConfStatus { + MPU9250_CONF_UNINIT, + MPU9250_CONF_RESET, + MPU9250_CONF_USER_RESET, + MPU9250_CONF_PWR, + MPU9250_CONF_SD, + MPU9250_CONF_DLPF_ACCEL, + MPU9250_CONF_DLPF_GYRO, + MPU9250_CONF_GYRO, + MPU9250_CONF_ACCEL, + MPU9250_CONF_I2C_SLAVES, + MPU9250_CONF_INT_ENABLE, + MPU9250_CONF_DONE +}; + +/// Configuration function prototype +typedef void (*Mpu9250ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); + +/// function prototype for configuration of a single I2C slave +typedef bool_t (*Mpu9250I2cSlaveConfigure)(Mpu9250ConfigSet mpu_set, void* mpu); + +struct Mpu9250I2cSlave { + Mpu9250I2cSlaveConfigure configure; +}; + +struct Mpu9250Config { + uint8_t smplrt_div; ///< Sample rate divider + enum Mpu9250DLPFAccel dlpf_accel_cfg; ///< Digital Low Pass Filter for accelerometer + enum Mpu9250DLPFGyro dlpf_gyro_cfg; ///< Digital Low Pass Filter for gyroscope + enum Mpu9250GyroRanges gyro_range; ///< deg/s Range + enum Mpu9250AccelRanges accel_range; ///< g Range + bool_t drdy_int_enable; ///< Enable Data Ready Interrupt + uint8_t clk_sel; ///< Clock select + uint8_t nb_bytes; ///< number of bytes to read starting with MPU9250_REG_INT_STATUS + enum Mpu9250ConfStatus init_status; ///< init status + bool_t initialized; ///< config done flag + + /** Bypass MPU I2C. + * Only effective if using the I2C implementation. + */ + bool_t i2c_bypass; + + uint8_t nb_slaves; ///< number of used I2C slaves + struct Mpu9250I2cSlave slaves[5]; ///< I2C slaves + enum Mpu9250MstClk i2c_mst_clk; ///< MPU I2C master clock speed + uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate +}; + +extern void mpu9250_set_default_config(struct Mpu9250Config *c); + +/// Configuration sequence called once before normal use +extern void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config); + +/** + * Configure I2C slaves of the MPU. + * This is I2C/SPI implementation specific. + * @param mpu_set configuration function + * @param mpu Mpu9250Spi or Mpu9250I2c peripheral + * @return TRUE when all slaves are configured + */ +extern bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu); + +#endif // MPU9250_H diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c new file mode 100644 index 00000000000..d46efbe71c6 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250_i2c.c + * + * Driver for the MPU-9250 using I2C. + * + */ + +#include "peripherals/mpu9250_i2c.h" + +void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + mpu->i2c_p = i2c_p; + + /* slave address */ + mpu->i2c_trans.slave_addr = addr; + /* set inital status: Success or Done */ + mpu->i2c_trans.status = I2CTransDone; + + /* set default MPU9250 config options */ + mpu9250_set_default_config(&(mpu->config)); + + mpu->data_available = FALSE; + mpu->config.initialized = FALSE; + mpu->config.init_status = MPU9250_CONF_UNINIT; + + mpu->slave_init_status = MPU9250_I2C_CONF_UNINIT; +} + + +static void mpu9250_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { + struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); + mpu_i2c->i2c_trans.buf[0] = _reg; + mpu_i2c->i2c_trans.buf[1] = _val; + i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2); +} + +// Configuration function called once before normal use +void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu) +{ + if (mpu->config.init_status == MPU9250_CONF_UNINIT) { + mpu->config.init_status++; + if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) { + mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config)); + } + } +} + +void mpu9250_i2c_read(struct Mpu9250_I2c *mpu) +{ + if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) { + /* set read bit and multiple byte bit, then address */ + mpu->i2c_trans.buf[0] = MPU9250_REG_INT_STATUS; + i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes); + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) + +void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) +{ + if (mpu->config.initialized) { + if (mpu->i2c_trans.status == I2CTransFailed) { + mpu->i2c_trans.status = I2CTransDone; + } + else if (mpu->i2c_trans.status == I2CTransSuccess) { + // Successfull reading + if (bit_is_set(mpu->i2c_trans.buf[0], 0)) { + // new data + mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1); + mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3); + mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5); + mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9); + mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11); + mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13); + + // if we are reading slaves through the mpu, copy the ext_sens_data + if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) + memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + + mpu->data_available = TRUE; + } + mpu->i2c_trans.status = I2CTransDone; + } + } + else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized + switch (mpu->i2c_trans.status) { + case I2CTransFailed: + mpu->config.init_status--; // Retry config (TODO max retry) + case I2CTransSuccess: + case I2CTransDone: + mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config)); + if (mpu->config.initialized) + mpu->i2c_trans.status = I2CTransDone; + break; + default: + break; + } + } +} + +/** @todo: only one slave so far. */ +bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) +{ + struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); + + if (mpu_i2c->slave_init_status == MPU9250_I2C_CONF_UNINIT) + mpu_i2c->slave_init_status++; + + switch (mpu_i2c->slave_init_status) { + case MPU9250_I2C_CONF_I2C_MST_DIS: + mpu_set(mpu, MPU9250_REG_USER_CTRL, 0); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_I2C_BYPASS_EN: + /* switch to I2C passthrough */ + mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (1<<1)); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_SLAVES_CONFIGURE: + /* configure each slave. TODO: currently only one */ + if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_I2C_BYPASS_DIS: + if (mpu_i2c->config.i2c_bypass) { + /* if bypassing I2C skip MPU I2C master setup */ + mpu_i2c->slave_init_status = MPU9250_I2C_CONF_DONE; + } + else { + /* disable I2C passthrough again */ + mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (0<<1)); + mpu_i2c->slave_init_status++; + } + break; + case MPU9250_I2C_CONF_I2C_MST_CLK: + /* configure MPU I2C master clock and stop/start between slave reads */ + mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, + ((1<<4) | mpu_i2c->config.i2c_mst_clk)); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_I2C_MST_DELAY: + /* Set I2C slaves delayed sample rate */ + mpu_set(mpu, MPU9250_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_I2C_SMPLRT: + /* I2C slave0 sample rate/2 = 100/2 = 50Hz */ + mpu_set(mpu, MPU9250_REG_I2C_SLV4_CTRL, 0); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_I2C_MST_EN: + /* enable internal I2C master */ + mpu_set(mpu, MPU9250_REG_USER_CTRL, (1 << MPU9250_I2C_MST_EN)); + mpu_i2c->slave_init_status++; + break; + case MPU9250_I2C_CONF_DONE: + return TRUE; + default: + break; + } + return FALSE; +} diff --git a/sw/airborne/peripherals/mpu9250_i2c.h b/sw/airborne/peripherals/mpu9250_i2c.h new file mode 100644 index 00000000000..3c1e4842f4e --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_i2c.h @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250_i2c.h + * + * Driver for the MPU-9250 using I2C. + */ + +#ifndef MPU9250_I2C_H +#define MPU9250_I2C_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/i2c.h" + +/* Include common MPU9250 options and definitions */ +#include "peripherals/mpu9250.h" + + +#define MPU9250_BUFFER_EXT_LEN 16 + +enum Mpu9250I2cSlaveInitStatus { + MPU9250_I2C_CONF_UNINIT, + MPU9250_I2C_CONF_I2C_MST_DIS, + MPU9250_I2C_CONF_I2C_BYPASS_EN, + MPU9250_I2C_CONF_SLAVES_CONFIGURE, + MPU9250_I2C_CONF_I2C_BYPASS_DIS, + MPU9250_I2C_CONF_I2C_MST_CLK, + MPU9250_I2C_CONF_I2C_MST_DELAY, + MPU9250_I2C_CONF_I2C_SMPLRT, + MPU9250_I2C_CONF_I2C_MST_EN, + MPU9250_I2C_CONF_DONE +}; + +struct Mpu9250_I2c { + struct i2c_periph *i2c_p; + struct i2c_transaction i2c_trans; + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< accel data vector in accel coordinate system + int16_t value[3]; ///< accel data values accessible by channel index + } data_accel; + union { + struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system + int16_t value[3]; ///< rates data values accessible by channel index + } data_rates; + uint8_t data_ext[MPU9250_BUFFER_EXT_LEN]; + struct Mpu9250Config config; + enum Mpu9250I2cSlaveInitStatus slave_init_status; +}; + +// Functions +extern void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr); +extern void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu); +extern void mpu9250_i2c_read(struct Mpu9250_I2c *mpu); +extern void mpu9250_i2c_event(struct Mpu9250_I2c *mpu); + +/// convenience function: read or start configuration if not already initialized +static inline void mpu9250_i2c_periodic(struct Mpu9250_I2c *mpu) { + if (mpu->config.initialized) + mpu9250_i2c_read(mpu); + else + mpu9250_i2c_start_configure(mpu); +} + +#endif // MPU9250_I2C_H diff --git a/sw/airborne/peripherals/mpu9250_regs.h b/sw/airborne/peripherals/mpu9250_regs.h new file mode 100644 index 00000000000..ab452fd18e2 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_regs.h @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2010-2013 The Paparazzi Team + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250_regs.h + * + * Register and address definitions for MPU-9250. + */ + +#ifndef MPU9250_REGS_H +#define MPU9250_REGS_H + +/* default I2C address */ +#define MPU9250_ADDR 0xD0 +#define MPU9250_ADDR_ALT 0xD2 + +#define MPU9250_SPI_READ 0x80 + +// Power and Interface +#define MPU9250_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000 +#define MPU9250_REG_USER_CTRL 0x6A +#define MPU9250_REG_PWR_MGMT_1 0x6B +#define MPU9250_REG_PWR_MGMT_2 0x6C + +// FIFO +#define MPU9250_REG_FIFO_EN 0x23 +#define MPU9250_REG_FIFO_COUNT_H 0x72 +#define MPU9250_REG_FIFO_COUNT_L 0x73 +#define MPU9250_REG_FIFO_R_W 0x74 + +// Measurement Settings +#define MPU9250_REG_SMPLRT_DIV 0x19 +#define MPU9250_REG_CONFIG 0x1A +#define MPU9250_REG_GYRO_CONFIG 0x1B +#define MPU9250_REG_ACCEL_CONFIG 0x1C +#define MPU9250_REG_ACCEL_CONFIG_2 0x1D + + +// I2C Slave settings +#define MPU9250_REG_I2C_MST_CTRL 0x24 +#define MPU9250_REG_I2C_MST_STATUS 0x36 +#define MPU9250_REG_I2C_MST_DELAY 0x67 +// Slave 0 +#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr +#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg +#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits +#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO +// Slave 1 +#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr +#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg +#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits +#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO +// Slave 2 +#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr +#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg +#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits +#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO +// Slave 3 +#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr +#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg +#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits +#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO +// Slave 4 - special +#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr +#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg +#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO +#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits +#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI + +// Interrupt +#define MPU9250_REG_INT_PIN_CFG 0x37 +#define MPU9250_REG_INT_ENABLE 0x38 +#define MPU9250_REG_INT_STATUS 0x3A + +// Accelero +#define MPU9250_REG_ACCEL_XOUT_H 0x3B +#define MPU9250_REG_ACCEL_XOUT_L 0x3C +#define MPU9250_REG_ACCEL_YOUT_H 0x3D +#define MPU9250_REG_ACCEL_YOUT_L 0x3E +#define MPU9250_REG_ACCEL_ZOUT_H 0x3F +#define MPU9250_REG_ACCEL_ZOUT_L 0x40 + +// Temperature +#define MPU9250_REG_TEMP_OUT_H 0x41 +#define MPU9250_REG_TEMP_OUT_L 0x42 + +// Gyro +#define MPU9250_REG_GYRO_XOUT_H 0x43 +#define MPU9250_REG_GYRO_XOUT_L 0x44 +#define MPU9250_REG_GYRO_YOUT_H 0x45 +#define MPU9250_REG_GYRO_YOUT_L 0x46 +#define MPU9250_REG_GYRO_ZOUT_H 0x47 +#define MPU9250_REG_GYRO_ZOUT_L 0x48 + +// External Sensor Data +#define MPU9250_EXT_SENS_DATA 0x49 +#define MPU9250_EXT_SENS_DATA_SIZE 24 + + +#define MPU9250_REG_WHO_AM_I 0x75 +#define MPU9250_WHOAMI_REPLY 0x68 + +// Bit positions +#define MPU9250_I2C_BYPASS_EN 1 + +// in MPU9250_REG_USER_CTRL +#define MPU9250_SIG_COND_RESET 0 +#define MPU9250_I2C_MST_RESET 1 +#define MPU9250_FIFO_RESET 2 +#define MPU9250_I2C_IF_DIS 4 +#define MPU9250_I2C_MST_EN 5 +#define MPU9250_FIFO_EN 6 + +// in MPU9250_REG_I2C_MST_STATUS +#define MPU9250_I2C_SLV4_DONE 6 + +/** Digital Low Pass Filter Options + */ +enum Mpu9250DLPFGyro { + MPU9250_DLPF__GYRO_250HZ = 0x0, // internal sampling rate 8kHz + MPU9250_DLPF_GYRO_184HZ = 0x1, // internal sampling rate 1kHz + MPU9250_DLPF_GYRO_92HZ = 0x2, + MPU9250_DLPF_GYRO_41HZ = 0x3, + MPU9250_DLPF_GYRO_20HZ = 0x4, + MPU9250_DLPF_GYRO_10HZ = 0x5, + MPU9250_DLPF_GYRO_05HZ = 0x6 +}; + + +enum Mpu9250DLPFAccel { + MPU9250_DLPF_ACCEL_460HZ = 0x0, // internal sampling rate 8kHz + MPU9250_DLPF_ACCEL_184HZ = 0x1, // internal sampling rate 1kHz + MPU9250_DLPF_ACCEL_92HZ = 0x2, + MPU9250_DLPF_ACCEL_41HZ = 0x3, + MPU9250_DLPF_ACCEL_20HZ = 0x4, + MPU9250_DLPF_ACCEL_10HZ = 0x5, + MPU9250_DLPF_ACCEL_05HZ = 0x6 +}; +/** + * Selectable gyro range + */ +enum Mpu9250GyroRanges { + MPU9250_GYRO_RANGE_250 = 0x00, + MPU9250_GYRO_RANGE_500 = 0x01, + MPU9250_GYRO_RANGE_1000 = 0x02, + MPU9250_GYRO_RANGE_2000 = 0x03 +}; + +/** + * Selectable accel range + */ +enum Mpu9250AccelRanges { + MPU9250_ACCEL_RANGE_2G = 0x00, + MPU9250_ACCEL_RANGE_4G = 0x01, + MPU9250_ACCEL_RANGE_8G = 0x02, + MPU9250_ACCEL_RANGE_16G = 0x03 +}; + +/** + * I2C Master clock + */ +enum Mpu9250MstClk { + MPU9250_MST_CLK_500KHZ = 0x9, + MPU9250_MST_CLK_471KHZ = 0xA, + MPU9250_MST_CLK_444KHZ = 0xB, + MPU9250_MST_CLK_421KHZ = 0xC, + MPU9250_MST_CLK_400KHZ = 0xD, + MPU9250_MST_CLK_381KHZ = 0xE, + MPU9250_MST_CLK_364KHZ = 0xF, + MPU9250_MST_CLK_348KHZ = 0x0, + MPU9250_MST_CLK_333KHZ = 0x1, + MPU9250_MST_CLK_320KHZ = 0x2, + MPU9250_MST_CLK_308KHZ = 0x3, + MPU9250_MST_CLK_296KHZ = 0x4, + MPU9250_MST_CLK_286KHZ = 0x5, + MPU9250_MST_CLK_276KHZ = 0x6, + MPU9250_MST_CLK_267KHZ = 0x7, + MPU9250_MST_CLK_258KHZ = 0x8 +}; + +#endif /* MPU9250_REGS_H */ diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c new file mode 100644 index 00000000000..0fff4d53305 --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250_spi.c + * + * Driver for the MPU-9250 using SPI. + * + */ + +#include "peripherals/mpu9250_spi.h" + +void mpu9250_spi_init(struct Mpu9250_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx) +{ + /* set spi_peripheral */ + mpu->spi_p = spi_p; + + /* configure spi transaction */ + mpu->spi_trans.cpol = SPICpolIdleHigh; + mpu->spi_trans.cpha = SPICphaEdge2; + mpu->spi_trans.dss = SPIDss8bit; + mpu->spi_trans.bitorder = SPIMSBFirst; + mpu->spi_trans.cdiv = SPIDiv64; + + mpu->spi_trans.select = SPISelectUnselect; + mpu->spi_trans.slave_idx = slave_idx; + mpu->spi_trans.output_length = 2; + mpu->spi_trans.input_length = MPU9250_BUFFER_LEN; + mpu->spi_trans.before_cb = NULL; + mpu->spi_trans.after_cb = NULL; + mpu->spi_trans.input_buf = &(mpu->rx_buf[0]); + mpu->spi_trans.output_buf = &(mpu->tx_buf[0]); + + /* set inital status: Success or Done */ + mpu->spi_trans.status = SPITransDone; + + /* set default MPU9250 config options */ + mpu9250_set_default_config(&(mpu->config)); + + mpu->data_available = FALSE; + mpu->config.initialized = FALSE; + mpu->config.init_status = MPU9250_CONF_UNINIT; + + mpu->slave_init_status = MPU9250_SPI_CONF_UNINIT; +} + + +static void mpu9250_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { + struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_Spi*)(mpu); + mpu_spi->spi_trans.output_length = 2; + mpu_spi->spi_trans.input_length = 0; + mpu_spi->tx_buf[0] = _reg; + mpu_spi->tx_buf[1] = _val; + spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans)); +} + +// Configuration function called once before normal use +void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu) +{ + if (mpu->config.init_status == MPU9250_CONF_UNINIT) { + mpu->config.init_status++; + if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) { + mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config)); + } + } +} + +void mpu9250_spi_read(struct Mpu9250_Spi *mpu) +{ + if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) { + mpu->spi_trans.output_length = 1; + mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes; + /* set read bit and multiple byte bit, then address */ + mpu->tx_buf[0] = MPU9250_REG_INT_STATUS | MPU9250_SPI_READ; + spi_submit(mpu->spi_p, &(mpu->spi_trans)); + } +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) + +void mpu9250_spi_event(struct Mpu9250_Spi *mpu) +{ + if (mpu->config.initialized) { + if (mpu->spi_trans.status == SPITransFailed) { + mpu->spi_trans.status = SPITransDone; + } + else if (mpu->spi_trans.status == SPITransSuccess) { + // Successfull reading + if (bit_is_set(mpu->rx_buf[1], 0)) { + // new data + mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2); + mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4); + mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6); + mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10); + mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12); + mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14); + + // if we are reading slaves, copy the ext_sens_data + if (mpu->config.nb_slaves > 0) + memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + + mpu->data_available = TRUE; + } + mpu->spi_trans.status = SPITransDone; + } + } + else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized + switch (mpu->spi_trans.status) { + case SPITransFailed: + mpu->config.init_status--; // Retry config (TODO max retry) + case SPITransSuccess: + case SPITransDone: + mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config)); + if (mpu->config.initialized) + mpu->spi_trans.status = SPITransDone; + break; + default: + break; + } + } +} + +/** @todo: only one slave so far. */ +bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) +{ + struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_Spi*)(mpu); + + if (mpu_spi->slave_init_status == MPU9250_SPI_CONF_UNINIT) + mpu_spi->slave_init_status++; + + switch (mpu_spi->slave_init_status) { + case MPU9250_SPI_CONF_I2C_MST_CLK: + /* configure MPU I2C master clock and stop/start between slave reads */ + mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk)); + mpu_spi->slave_init_status++; + break; + case MPU9250_SPI_CONF_I2C_MST_DELAY: + /* Set I2C slaves delayed sample rate */ + mpu_set(mpu, MPU9250_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay); + mpu_spi->slave_init_status++; + break; + case MPU9250_SPI_CONF_I2C_MST_EN: + /* enable internal I2C master and disable primary I2C interface */ + mpu_set(mpu, MPU9250_REG_USER_CTRL, ((1 << MPU9250_I2C_IF_DIS) | + (1 << MPU9250_I2C_MST_EN))); + mpu_spi->slave_init_status++; + break; + case MPU9250_SPI_CONF_SLAVES_CONFIGURE: + /* configure first slave, only one slave supported so far */ + if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) + mpu_spi->slave_init_status++; + break; + case MPU9250_SPI_CONF_DONE: + return TRUE; + default: + break; + } + return FALSE; +} diff --git a/sw/airborne/peripherals/mpu9250_spi.h b/sw/airborne/peripherals/mpu9250_spi.h new file mode 100644 index 00000000000..d8135ba41be --- /dev/null +++ b/sw/airborne/peripherals/mpu9250_spi.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2013 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/mpu9250_spi.h + * + * Driver for the MPU-9250 using SPI. + */ + +#ifndef MPU9250_SPI_H +#define MPU9250_SPI_H + +#include "std.h" +#include "math/pprz_algebra_int.h" +#include "mcu_periph/spi.h" + +/* Include common MPU9250 options and definitions */ +#include "peripherals/mpu9250.h" + + +#define MPU9250_BUFFER_LEN 32 +#define MPU9250_BUFFER_EXT_LEN 16 + +enum Mpu9250SpiSlaveInitStatus { + MPU9250_SPI_CONF_UNINIT, + MPU9250_SPI_CONF_I2C_MST_CLK, + MPU9250_SPI_CONF_I2C_MST_DELAY, + MPU9250_SPI_CONF_I2C_MST_EN, + MPU9250_SPI_CONF_SLAVES_CONFIGURE, + MPU9250_SPI_CONF_DONE +}; + +struct Mpu9250_Spi { + struct spi_periph *spi_p; + struct spi_transaction spi_trans; + volatile uint8_t tx_buf[2]; + volatile uint8_t rx_buf[MPU9250_BUFFER_LEN]; + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< accel data vector in accel coordinate system + int16_t value[3]; ///< accel data values accessible by channel index + } data_accel; + union { + struct Int16Rates rates; ///< rates data as angular rates in gyro coordinate system + int16_t value[3]; ///< rates data values accessible by channel index + } data_rates; + uint8_t data_ext[MPU9250_BUFFER_EXT_LEN]; + struct Mpu9250Config config; + enum Mpu9250SpiSlaveInitStatus slave_init_status; +}; + +// Functions +extern void mpu9250_spi_init(struct Mpu9250_Spi *mpu, struct spi_periph *spi_p, uint8_t addr); +extern void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu); +extern void mpu9250_spi_read(struct Mpu9250_Spi *mpu); +extern void mpu9250_spi_event(struct Mpu9250_Spi *mpu); + +/// convenience function: read or start configuration if not already initialized +static inline void mpu9250_spi_periodic(struct Mpu9250_Spi *mpu) { + if (mpu->config.initialized) + mpu9250_spi_read(mpu); + else + mpu9250_spi_start_configure(mpu); +} + +#endif // MPU9250_SPI_H From 44e02c4152a0ccf56af6127c3e5b0ddcc3991d70 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sun, 16 Nov 2014 20:15:34 +0100 Subject: [PATCH 27/86] [uart] Fix weak handler --- sw/airborne/arch/omap/mcu_periph/uart_arch.c | 66 +++++++++++++++++-- sw/airborne/arch/omap/mcu_periph/uart_arch.h | 13 ---- sw/airborne/firmwares/fixedwing/main_ap.c | 2 + sw/airborne/firmwares/fixedwing/main_fbw.c | 3 + sw/airborne/firmwares/rotorcraft/main.c | 3 + sw/airborne/mcu_periph/uart.c | 24 +++++++ sw/airborne/mcu_periph/uart.h | 8 +++ sw/airborne/subsystems/ahrs.c | 1 - sw/airborne/subsystems/imu.c | 1 - sw/airborne/subsystems/imu.h | 2 +- sw/airborne/subsystems/imu/imu_ardrone2_raw.c | 10 --- sw/airborne/subsystems/imu/imu_ardrone2_raw.h | 3 - sw/airborne/subsystems/ins.c | 1 - sw/include/std.h | 2 + 14 files changed, 103 insertions(+), 36 deletions(-) diff --git a/sw/airborne/arch/omap/mcu_periph/uart_arch.c b/sw/airborne/arch/omap/mcu_periph/uart_arch.c index 5e2b0564e47..1cfaf048983 100644 --- a/sw/airborne/arch/omap/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/uart_arch.c @@ -52,7 +52,7 @@ void uart_periph_set_baudrate(struct uart_periph* periph, uint32_t baud) { //TODO: set device name in application and pass as argument // FIXME: paparazzi baud is 9600 for B9600 while open_raw needs 12 for B9600 - printf("opening %s on uart0 at termios.h baud value=%d\n", periph->dev, baud); + // /printf("opening %s on uart0 at termios.h baud value=%d\n", periph->dev, baud); int ret = serial_port_open_raw(port,periph->dev, baud); if (ret != 0) { @@ -118,22 +118,18 @@ static inline void uart_handler(struct uart_periph* periph) { } #if USE_UART0 - void uart0_init( void ) { uart_periph_init(&uart0); strcpy(uart0.dev, UART0_DEV); uart_periph_set_baudrate(&uart0, UART0_BAUD); } - void uart0_handler(void) { uart_handler(&uart0); } - #endif /* USE_UART0 */ #if USE_UART1 - void uart1_init( void ) { uart_periph_init(&uart1); strcpy(uart1.dev, UART1_DEV); @@ -143,6 +139,64 @@ void uart1_init( void ) { void uart1_handler(void) { uart_handler(&uart1); } - #endif /* USE_UART1 */ +#if USE_UART2 +void uart2_init(void) { + uart_periph_init(&uart2); + strcpy(uart2.dev, UART2_DEV); + uart_periph_set_baudrate(&uart2, UART2_BAUD); +} + +void uart2_handler(void) { + uart_handler(&uart2); +} +#endif /* USE_UART2 */ + +#if USE_UART3 +void uart3_init(void) { + uart_periph_init(&uart3); + strcpy(uart3.dev, UART3_DEV); + uart_periph_set_baudrate(&uart3, UART3_BAUD); +} + +void uart3_handler(void) { + uart_handler(&uart3); +} +#endif /* USE_UART3 */ + +#if USE_UART4 +void uart4_init(void) { + uart_periph_init(&uart4); + strcpy(uart4.dev, UART4_DEV); + uart_periph_set_baudrate(&uart4, UART4_BAUD); +} + +void uart4_handler(void) { + uart_handler(&uart4); +} +#endif /* USE_UART4 */ + +#if USE_UART5 +void uart5_init(void) { + uart_periph_init(&uart5); + strcpy(uart5.dev, UART5_DEV); + uart_periph_set_baudrate(&uart5, UART5_BAUD); +} + +void uart5_handler(void) { + uart_handler(&uart5); +} +#endif /* USE_UART5 */ + +#if USE_UART6 +void uart6_init(void) { + uart_periph_init(&uart6); + strcpy(uart6.dev, UART6_DEV); + uart_periph_set_baudrate(&uart6, UART6_BAUD); +} + +void uart6_handler(void) { + uart_handler(&uart6); +} +#endif /* USE_UART6 */ diff --git a/sw/airborne/arch/omap/mcu_periph/uart_arch.h b/sw/airborne/arch/omap/mcu_periph/uart_arch.h index 9577170577a..9e3a4b38965 100644 --- a/sw/airborne/arch/omap/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/omap/mcu_periph/uart_arch.h @@ -49,17 +49,4 @@ static inline int uart_speed(int def) { } #define UART_SPEED(_def) uart_speed(_def) -#define UART1_irq_handler usart1_irq_handler -#define UART2_irq_handler usart2_irq_handler -#define UART3_irq_handler usart3_irq_handler -#define UART5_irq_handler usart5_irq_handler - -#if USE_UART0 -extern void uart0_handler(void); -#endif - -#if USE_UART1 -extern void uart1_handler(void); -#endif - #endif /* UART_ARCH_H */ diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index dfb73f34d57..586f38b8834 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -652,6 +652,8 @@ void event_task_ap( void ) { #if USE_I2C0 || USE_I2C1 || USE_I2C2 || USE_I2C3 i2c_event(); #endif + + uart_event(); #endif #if USE_AHRS && USE_IMU diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index cb43ca4985a..edc520c8d7c 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -43,6 +43,7 @@ #include "firmwares/fixedwing/autopilot.h" #include "paparazzi.h" #include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" @@ -188,6 +189,8 @@ void event_task_fbw( void) { i2c_event(); #endif + uart_event(); + #ifdef INTER_MCU #if defined MCU_SPI_LINK | defined MCU_UART_LINK link_mcu_event_task(); diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 9d1efe88934..d90ec9d50cb 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -34,6 +34,7 @@ #include "mcu.h" #include "mcu_periph/sys_time.h" #include "mcu_periph/i2c.h" +#include "mcu_periph/uart.h" #if USE_UDP #include "mcu_periph/udp.h" #endif @@ -274,6 +275,8 @@ STATIC_INLINE void main_event( void ) { i2c_event(); + uart_event(); + #if USE_UDP udp_event(); #endif diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index ffb926d1f13..69a57179bd3 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -212,3 +212,27 @@ uint8_t uart_getch(struct uart_periph* p) { p->rx_extract_idx = (p->rx_extract_idx + 1) % UART_RX_BUFFER_SIZE; return ret; } + +void WEAK uart_event(void) { +#if USE_UART0 + uart0_handler(); +#endif +#if USE_UART1 + uart1_handler(); +#endif +#if USE_UART2 + uart2_handler(); +#endif +#if USE_UART3 + uart3_handler(); +#endif +#if USE_UART4 + uart4_handler(); +#endif +#if USE_UART5 + uart5_handler(); +#endif +#if USE_UART6 + uart6_handler(); +#endif +} diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 3c970f1c5b7..64dfd879fb9 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -82,6 +82,7 @@ extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_ extern void uart_transmit(struct uart_periph* p, uint8_t data); extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len); extern uint8_t uart_getch(struct uart_periph* p); +extern void WEAK uart_event(void); /** * Check UART for available chars in receive buffer. @@ -98,6 +99,7 @@ static inline uint16_t uart_char_available(struct uart_periph* p) { #if USE_UART0 extern struct uart_periph uart0; extern void uart0_init(void); +extern void WEAK uart0_handler(void); #define UART0Init() uart_periph_init(&uart0) #define UART0CheckFreeSpace(_x) uart_check_free_space(&uart0, _x) @@ -114,6 +116,7 @@ extern void uart0_init(void); #if USE_UART1 extern struct uart_periph uart1; extern void uart1_init(void); +extern void WEAK uart1_handler(void); #define UART1Init() uart_periph_init(&uart1) #define UART1CheckFreeSpace(_x) uart_check_free_space(&uart1, _x) @@ -130,6 +133,7 @@ extern void uart1_init(void); #if USE_UART2 extern struct uart_periph uart2; extern void uart2_init(void); +extern void WEAK uart2_handler(void); #define UART2Init() uart_periph_init(&uart2) #define UART2CheckFreeSpace(_x) uart_check_free_space(&uart2, _x) @@ -146,6 +150,7 @@ extern void uart2_init(void); #if USE_UART3 extern struct uart_periph uart3; extern void uart3_init(void); +extern void WEAK uart3_handler(void); #define UART3Init() uart_periph_init(&uart3) #define UART3CheckFreeSpace(_x) uart_check_free_space(&uart3, _x) @@ -162,6 +167,7 @@ extern void uart3_init(void); #if USE_UART4 extern struct uart_periph uart4; extern void uart4_init(void); +extern void WEAK uart4_handler(void); #define UART4Init() uart_periph_init(&uart4) #define UART4CheckFreeSpace(_x) uart_check_free_space(&uart4, _x) @@ -178,6 +184,7 @@ extern void uart4_init(void); #if USE_UART5 extern struct uart_periph uart5; extern void uart5_init(void); +extern void WEAK uart5_handler(void); #define UART5Init() uart_periph_init(&uart5) #define UART5CheckFreeSpace(_x) uart_check_free_space(&uart5, _x) @@ -194,6 +201,7 @@ extern void uart5_init(void); #if USE_UART6 extern struct uart_periph uart6; extern void uart6_init(void); +extern void WEAK uart6_handler(void); #define UART6Init() uart_periph_init(&uart6) #define UART6CheckFreeSpace(_x) uart_check_free_space(&uart6, _x) diff --git a/sw/airborne/subsystems/ahrs.c b/sw/airborne/subsystems/ahrs.c index c6695618618..229037001b0 100644 --- a/sw/airborne/subsystems/ahrs.c +++ b/sw/airborne/subsystems/ahrs.c @@ -29,7 +29,6 @@ struct Ahrs ahrs; -#define WEAK __attribute__((weak)) // weak functions, used if not explicitly provided by implementation void WEAK ahrs_propagate(float dt __attribute__((unused))) {} diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index ee2fdcd097d..b65a0a37cc7 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -224,7 +224,6 @@ void imu_SetBodyToImuCurrent(float set) { } -#define WEAK __attribute__((weak)) // weak functions, used if not explicitly provided by implementation void WEAK imu_scale_gyro(struct Imu* _imu) diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index 7cd951e50f6..38023a711af 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -34,7 +34,7 @@ /* must be defined by underlying hardware */ extern void imu_impl_init(void); -extern void imu_periodic(void); +extern void WEAK imu_periodic(void); //Optional /** abstract IMU interface providing fixed point interface */ struct Imu { diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c index 860de29c096..8fc8dc719bc 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c @@ -32,13 +32,3 @@ void imu_impl_init(void) { navdata_init(); } - -void imu_periodic(void) { -} - -void navdata_event(void) { - -#if USE_UART1 - uart1_handler(); -#endif -} diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index 98850a0efc7..3662ac6bce0 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -84,8 +84,6 @@ */ #include "subsystems/imu.h" -void navdata_event(void); - static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { navdata_update(); @@ -100,7 +98,6 @@ static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _a _accel_handler(); _mag_handler(); } - navdata_event(); } #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 0132207f044..6a52974f873 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -36,7 +36,6 @@ struct Ins ins; -#define WEAK __attribute__((weak)) // weak functions, used if not explicitly provided by implementation void WEAK ins_periodic(void) {} diff --git a/sw/include/std.h b/sw/include/std.h index bfd2d66c677..42f08799709 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -228,8 +228,10 @@ static inline bool_t str_equal(const char * a, const char * b) { #ifdef __GNUC__ # define UNUSED __attribute__((__unused__)) +# define WEAK __attribute__((weak)) #else # define UNUSED +# define WEAK #endif #endif /* STD_H */ From 23228936b904bf22377b8fd67ae6b36433f31a8c Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 16 Nov 2014 23:03:28 +0100 Subject: [PATCH 28/86] [mpu9250] add test module and imu subsystem --- .../subsystems/shared/imu_mpu9250.makefile | 54 +++++++ conf/modules/imu_mpu9250.xml | 27 ++++ sw/airborne/modules/sensors/imu_mpu9250.c | 71 +++++++++ sw/airborne/modules/sensors/imu_mpu9250.h | 41 +++++ sw/airborne/peripherals/mpu9250.h | 7 +- sw/airborne/peripherals/mpu9250_i2c.c | 27 +++- sw/airborne/subsystems/imu/imu_mpu9250.c | 142 ++++++++++++++++++ sw/airborne/subsystems/imu/imu_mpu9250.h | 101 +++++++++++++ 8 files changed, 462 insertions(+), 8 deletions(-) create mode 100644 conf/firmwares/subsystems/shared/imu_mpu9250.makefile create mode 100644 conf/modules/imu_mpu9250.xml create mode 100644 sw/airborne/modules/sensors/imu_mpu9250.c create mode 100644 sw/airborne/modules/sensors/imu_mpu9250.h create mode 100644 sw/airborne/subsystems/imu/imu_mpu9250.c create mode 100644 sw/airborne/subsystems/imu/imu_mpu9250.h diff --git a/conf/firmwares/subsystems/shared/imu_mpu9250.makefile b/conf/firmwares/subsystems/shared/imu_mpu9250.makefile new file mode 100644 index 00000000000..c7fe3ae54e4 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_mpu9250.makefile @@ -0,0 +1,54 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# MPU9250 IMU via I2C +# + +# for fixedwing firmware and ap only +ifeq ($(TARGET), ap) + IMU_MPU9250_CFLAGS = -DUSE_IMU +endif + +IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250.h\" +IMU_MPU9250_SRCS = $(SRC_SUBSYSTEMS)/imu.c +IMU_MPU9250_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250.c +IMU_MPU9250_SRCS += peripherals/mpu9250.c +IMU_MPU9250_SRCS += peripherals/mpu9250_i2c.c + +# Magnetometer +#IMU_MPU9250_SRCS += peripherals/ak8963.c + + +# set default i2c bus +ifeq ($(ARCH), lpc21) +MPU9250_I2C_DEV ?= i2c0 +else ifeq ($(ARCH), stm32) +MPU9250_I2C_DEV ?= i2c2 +endif + +ifeq ($(TARGET), ap) +ifndef MPU9250_I2C_DEV +$(error Error: MPU9250_I2C_DEV not configured!) +endif +endif + +# convert i2cx to upper/lower case +MPU9250_I2C_DEV_UPPER=$(shell echo $(MPU9250_I2C_DEV) | tr a-z A-Z) +MPU9250_I2C_DEV_LOWER=$(shell echo $(MPU9250_I2C_DEV) | tr A-Z a-z) + +IMU_MPU9250_CFLAGS += -DIMU_MPU9250_I2C_DEV=$(MPU9250_I2C_DEV_LOWER) +IMU_MPU9250_CFLAGS += -DUSE_$(MPU9250_I2C_DEV_UPPER) + + +# Keep CFLAGS/Srcs for imu in separate expression so we can assign it to other targets + +ap.CFLAGS += $(IMU_MPU9250_CFLAGS) +ap.srcs += $(IMU_MPU9250_SRCS) + +test_imu.CFLAGS += $(IMU_MPU9250_CFLAGS) +test_imu.srcs += $(IMU_MPU9250_SRCS) + + +# +# NPS simulator +# +include $(CFG_SHARED)/imu_nps.makefile diff --git a/conf/modules/imu_mpu9250.xml b/conf/modules/imu_mpu9250.xml new file mode 100644 index 00000000000..5454c1eda95 --- /dev/null +++ b/conf/modules/imu_mpu9250.xml @@ -0,0 +1,27 @@ + + + + + + Test module for the mpu9250 with I2C + Report RAW values on telemetry + + + + +
+ +
+ + + + + + + + + + + + + diff --git a/sw/airborne/modules/sensors/imu_mpu9250.c b/sw/airborne/modules/sensors/imu_mpu9250.c new file mode 100644 index 00000000000..5ec11265cf6 --- /dev/null +++ b/sw/airborne/modules/sensors/imu_mpu9250.c @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/sensors/imu_mpu9250.c" + * @author Gautier Hattenberger + * + * Test module for the mpu9250 + */ + +#include "modules/sensors/imu_mpu9250.h" + +// Default I2C address +#ifndef IMU_MPU9250_ADDR +#define IMU_MPU9250_ADDR MPU9250_ADDR +#endif + +struct Mpu9250_I2c mpu9250; + +void imu_mpu9250_init(void) +{ + mpu9250_i2c_init(&mpu9250, &(IMU_MPU9250_I2C_DEV), IMU_MPU9250_ADDR); +} + +void imu_mpu9250_periodic(void) +{ + mpu9250_i2c_periodic(&mpu9250); +} + +void imu_mpu9250_event(void) +{ + mpu9250_i2c_event(&mpu9250); +} + +#include "math/pprz_algebra_int.h" +#include "subsystems/datalink/downlink.h" + +void imu_mpu9250_report(void) +{ + struct Int32Vect3 accel = { + (int32_t)(mpu9250.data_accel.vect.x), + (int32_t)(mpu9250.data_accel.vect.y), + (int32_t)(mpu9250.data_accel.vect.z) + }; + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &accel.x, &accel.y, &accel.z); + + struct Int32Rates rates = { + (int32_t)(mpu9250.data_rates.rates.p), + (int32_t)(mpu9250.data_rates.rates.q), + (int32_t)(mpu9250.data_rates.rates.r) + }; + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &rates.p, &rates.q, &rates.r); +} + diff --git a/sw/airborne/modules/sensors/imu_mpu9250.h b/sw/airborne/modules/sensors/imu_mpu9250.h new file mode 100644 index 00000000000..a5c272cf751 --- /dev/null +++ b/sw/airborne/modules/sensors/imu_mpu9250.h @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file "modules/sensors/imu_mpu9250.h" + * @author Gautier Hattenberger + * + * Test module for the mpu9250 + */ + +#ifndef IMU_MPU9250_H +#define IMU_MPU9250_H + +#include "peripherals/mpu9250_i2c.h" + +extern struct Mpu9250_I2c mpu9250; + +extern void imu_mpu9250_init(void); +extern void imu_mpu9250_periodic(void); +extern void imu_mpu9250_event(void); +extern void imu_mpu9250_report(void); + +#endif + diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 90d906c1964..2fcfed58bf2 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Gautier Hattenberger + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +14,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index d46efbe71c6..b995e12003c 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Gautier Hattenberger + * Copyright (C) 2014 Gautier Hattenberger * * This file is part of paparazzi. * @@ -14,9 +14,8 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** @@ -45,6 +44,14 @@ void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t mpu->config.initialized = FALSE; mpu->config.init_status = MPU9250_CONF_UNINIT; + /* mag is declared as slave to call the configure function, + * regardless if it is an actual MPU slave or passthrough + */ + //mpu->config.nb_slaves = 1; + /* set callback function to configure mag */ + //mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave; + + mpu->slave_init_status = MPU9250_I2C_CONF_UNINIT; } @@ -120,6 +127,18 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) } } +/** callback function to configure ak8963 mag + * @return TRUE if mag configuration finished + */ +//bool_t imu_mpu9250_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) +//{ +// ak8963_start_configure(&imu_mpu9250.akm); +// if (imu_mpu9250.akm.initialized) +// return TRUE; +// else +// return FALSE; +//} + /** @todo: only one slave so far. */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) { diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.c b/sw/airborne/subsystems/imu/imu_mpu9250.c new file mode 100644 index 00000000000..ea9f6179fe0 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_mpu9250.c @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/imu/imu_mpu9250.c + * + * IMU driver for the MPU9250 using I2C + * + */ + +#include "subsystems/imu.h" + +#include "mcu_periph/i2c.h" + +#if !defined IMU_MPU9250_GYRO_LOWPASS_FILTER && !defined IMU_MPU9250_ACCEL_LOWPASS_FILTER && !defined IMU_MPU9250_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ +#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ +#define IMU_MPU9250_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") +#elif PERIODIC_FREQUENCY == 512 +/* Accelerometer: Bandwidth 260Hz, Delay 0ms + * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz + */ +#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ +#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ +#define IMU_MPU9250_SMPLRT_DIV 3 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#endif +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV) +PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_LOWPASS_FILTER) +PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_LOWPASS_FILTER) + +#ifndef IMU_MPU9250_GYRO_RANGE +#define IMU_MPU9250_GYRO_RANGE MPU9250_GYRO_RANGE_1000 +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_GYRO_RANGE) + +#ifndef IMU_MPU9250_ACCEL_RANGE +#define IMU_MPU9250_ACCEL_RANGE MPU9250_ACCEL_RANGE_8G +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_ACCEL_RANGE) + +#ifndef IMU_MPU9250_I2C_ADDR +#define IMU_MPU9250_I2C_ADDR MPU9250_ADDR_ALT +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_I2C_ADDR) + +// Default channels order +#ifndef IMU_MPU9250_CHAN_X +#define IMU_MPU9250_CHAN_X 0 +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_X) +#ifndef IMU_MPU9250_CHAN_Y +#define IMU_MPU9250_CHAN_Y 1 +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_Y) +#ifndef IMU_MPU9250_CHAN_Z +#define IMU_MPU9250_CHAN_Z 2 +#endif +PRINT_CONFIG_VAR(IMU_MPU9250_CHAN_Z) + + +struct ImuMpu9250 imu_mpu9250; + +void imu_impl_init(void) +{ + /* MPU9250 */ + mpu9250_i2c_init(&imu_mpu9250.mpu, &(IMU_MPU9250_I2C_DEV), IMU_MPU9250_I2C_ADDR); + // change the default configuration + imu_mpu9250.mpu.config.smplrt_div = IMU_MPU9250_SMPLRT_DIV; + imu_mpu9250.mpu.config.dlpf_gyro_cfg = IMU_MPU9250_GYRO_LOWPASS_FILTER; + imu_mpu9250.mpu.config.dlpf_accel_cfg = IMU_MPU9250_ACCEL_LOWPASS_FILTER; + imu_mpu9250.mpu.config.gyro_range = IMU_MPU9250_GYRO_RANGE; + imu_mpu9250.mpu.config.accel_range = IMU_MPU9250_ACCEL_RANGE; + + imu_mpu9250.gyro_valid = FALSE; + imu_mpu9250.accel_valid = FALSE; + imu_mpu9250.mag_valid = FALSE; +} + +void imu_periodic(void) +{ + mpu9250_i2c_periodic(&imu_mpu9250.mpu); +} + +void imu_mpu9250_event(void) +{ + // If the MPU9250 I2C transaction has succeeded: convert the data + mpu9250_i2c_event(&imu_mpu9250.mpu); + + if (imu_mpu9250.mpu.data_available) { + // set channel order + struct Int32Vect3 accel = { + (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_X]), + (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Y]), + (int32_t)(imu_mpu9250.mpu.data_accel.value[IMU_MPU9250_CHAN_Z]) + }; + struct Int32Rates rates = { + (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_X]), + (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), + (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) + }; + //struct Int32Vect3 mag = { + // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_X]), + // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Y]), + // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Z]) + //}; + // unscaled vector + VECT3_COPY(imu.accel_unscaled, accel); + RATES_COPY(imu.gyro_unscaled, rates); + //VECT3_COPY(imu.mag_unscaled, mag); + + imu_mpu9250.mpu.data_available = FALSE; + imu_mpu9250.gyro_valid = TRUE; + imu_mpu9250.accel_valid = TRUE; + //imu_mpu9250.mag_valid = TRUE; + } + +} + diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.h b/sw/airborne/subsystems/imu/imu_mpu9250.h new file mode 100644 index 00000000000..42d0b8eff89 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_mpu9250.h @@ -0,0 +1,101 @@ +/* + * Copyright (C) 2014 Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/imu/imu_mpu9250.h + * + * IMU driver for the MPU9250 using I2C + * + */ + +#ifndef IMU_MPU9250_H +#define IMU_MPU9250_H + +#include "std.h" +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/mpu9250_i2c.h" + + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 1000 deg/s has 32.8 LSB/(deg/s) + * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/32.8 * pi/180 * 4096 = 2.17953 + I*/ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 2.17953 +#define IMU_GYRO_P_SENS_NUM 18271 +#define IMU_GYRO_P_SENS_DEN 8383 +#define IMU_GYRO_Q_SENS 2.17953 +#define IMU_GYRO_Q_SENS_NUM 18271 +#define IMU_GYRO_Q_SENS_DEN 8383 +#define IMU_GYRO_R_SENS 2.17953 +#define IMU_GYRO_R_SENS_NUM 18271 +#define IMU_GYRO_R_SENS_DEN 8383 +#endif + +/** default accel sensitivy from the datasheet + * MPU with 8g has 4096 LSB/g + * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 2.4525 +#define IMU_ACCEL_X_SENS_NUM 981 +#define IMU_ACCEL_X_SENS_DEN 400 +#define IMU_ACCEL_Y_SENS 2.4525 +#define IMU_ACCEL_Y_SENS_NUM 981 +#define IMU_ACCEL_Y_SENS_DEN 400 +#define IMU_ACCEL_Z_SENS 2.4525 +#define IMU_ACCEL_Z_SENS_NUM 981 +#define IMU_ACCEL_Z_SENS_DEN 400 +#endif + + +struct ImuMpu9250 { + volatile bool_t gyro_valid; + volatile bool_t accel_valid; + volatile bool_t mag_valid; + struct Mpu9250_I2c mpu; +}; + +extern struct ImuMpu9250 imu_mpu9250; + +extern void imu_mpu9250_event(void); + +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { + imu_mpu9250_event(); + if (imu_mpu9250.accel_valid) { + imu_mpu9250.accel_valid = FALSE; + _accel_handler(); + } + if (imu_mpu9250.mag_valid) { + imu_mpu9250.mag_valid = FALSE; + _mag_handler(); + } + if (imu_mpu9250.gyro_valid) { + imu_mpu9250.gyro_valid = FALSE; + _gyro_handler(); + } +} + +#endif /* IMU_MPU9250_H */ From a910746c4abf5fd94fc26e3284da52021bc5f9e6 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Fri, 14 Nov 2014 16:51:07 +0100 Subject: [PATCH 29/86] [sensors] Added the AKM 8963 Magnetometer --- sw/airborne/peripherals/ak8963.c | 139 ++++++++++++++++++++++++++ sw/airborne/peripherals/ak8963.h | 84 ++++++++++++++++ sw/airborne/peripherals/ak8963_regs.h | 65 ++++++++++++ 3 files changed, 288 insertions(+) create mode 100644 sw/airborne/peripherals/ak8963.c create mode 100644 sw/airborne/peripherals/ak8963.h create mode 100644 sw/airborne/peripherals/ak8963_regs.h diff --git a/sw/airborne/peripherals/ak8963.c b/sw/airborne/peripherals/ak8963.c new file mode 100644 index 00000000000..c9dd06ff7c5 --- /dev/null +++ b/sw/airborne/peripherals/ak8963.c @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/ak8963.c + * + * Driver for the AKM AK8963 magnetometer. + */ + +#include "peripherals/ak8963.h" +#include "std.h" + +/** + * Initialize AK8963 struct + */ +void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + ak->i2c_p = i2c_p; + /* set i2c address */ + ak->i2c_trans.slave_addr = addr; + ak->i2c_trans.status = I2CTransDone; + ak->initialized = FALSE; + ak->init_status = AK_CONF_UNINIT; + + // Soft reset the device + ak->i2c_trans.buf[0] = AK8963_REG_CNTL2; + ak->i2c_trans.buf[1] = 1; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); +} + +void ak8963_configure(struct Ak8963 *ak) +{ + // Only configure when not busy + if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed) { + return; + } + + // Only when succesfull continue with next + if (ak->i2c_trans.status == I2CTransSuccess) { + ak->init_status++; + } + + ak->i2c_trans.status = I2CTransDone; + switch (ak->init_status) { + + // Soft Reset the device + case AK_CONF_UNINIT: + ak->i2c_trans.buf[0] = AK8963_REG_CNTL2; + ak->i2c_trans.buf[1] = 1; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + // Set it to continious measuring mode 2 + case AK_CONF_MODE: + ak->i2c_trans.buf[0] = AK8963_REG_CNTL1; + ak->i2c_trans.buf[1] = AK8963_CNTL1_CM_2; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + // Initialization done + default: + ak->initialized = TRUE; + break; + } +} + +void ak8963_read(struct Ak8963 *ak) +{ + if (ak->status != AK_STATUS_IDLE) { + return; + } + + // Read the status register + ak->i2c_trans.buf[0] = AK8963_REG_ST1; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1); +} + +#define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8))) +void ak8963_event(struct Ak8963 *ak) +{ + if (!ak->initialized) { + return; + } + + switch (ak->status) { + case AK_STATUS_IDLE: + // When DRDY start reading + if (ak->i2c_trans.status == I2CTransSuccess && ak->i2c_trans.buf[0] & 1) { + ak->i2c_trans.buf[0] = AK8963_REG_HXL; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 6); + ak->status++; + } + break; + + case AK_STATUS_READ: + if (ak->i2c_trans.status == I2CTransSuccess) { + // Copy the data + ak->data.vect.x = Int16FromBuf(ak->i2c_trans.buf, 0); + ak->data.vect.y = Int16FromBuf(ak->i2c_trans.buf, 2); + ak->data.vect.z = Int16FromBuf(ak->i2c_trans.buf, 4); + + // Read second status register to be ready for reading again + ak->i2c_trans.buf[0] = AK8963_REG_ST2; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 1); + ak->status++; + break; + } + + ak->i2c_trans.buf[0] = AK8963_REG_HXL; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 6); + break; + + default: + // Goto idle + ak->data_available = TRUE; + ak->i2c_trans.status = I2CTransDone; + ak->status = AK_STATUS_IDLE; + break; + } +} diff --git a/sw/airborne/peripherals/ak8963.h b/sw/airborne/peripherals/ak8963.h new file mode 100644 index 00000000000..92db595306a --- /dev/null +++ b/sw/airborne/peripherals/ak8963.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/ak8963.c + * + * Driver for the AKM AK8963 magnetometer. + */ + +#ifndef AK8963_H +#define AK8963_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +/* Address and register definitions */ +#include "peripherals/ak8963_regs.h" + +/** Config status states */ +enum Ak8963ConfStatus { + AK_CONF_UNINIT, + AK_CONF_MODE, + AK_CONF_DONE +}; + +/** Normal status states */ +enum Ak8963Status { + AK_STATUS_IDLE, + AK_STATUS_READ, + AK_STATUS_DONE +}; + +/** Default Ak8963 structure */ +struct Ak8963 { + struct i2c_periph *i2c_p; ///< peripheral used for communcation + struct i2c_transaction i2c_trans; ///< i2c transaction used for communication with the ak8936 + bool_t initialized; ///< config done flag + + enum Ak8963Status status; ///< main status + enum Ak8963ConfStatus init_status; ///< init status + + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; +}; + +// Functions +extern void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr); +extern void ak8963_configure(struct Ak8963 *ak); +extern void ak8963_event(struct Ak8963 *ak); +extern void ak8963_read(struct Ak8963 *ak); + +/// convenience function: read or start configuration if not already initialized +static inline void ak8963_periodic(struct Ak8963 *ak) +{ + if (ak->initialized) { + ak8963_read(ak); + } else { + ak8963_configure(ak); + } +} + +#endif /* AK8963_H */ diff --git a/sw/airborne/peripherals/ak8963_regs.h b/sw/airborne/peripherals/ak8963_regs.h new file mode 100644 index 00000000000..dbb926a2fc1 --- /dev/null +++ b/sw/airborne/peripherals/ak8963_regs.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/ak8963.h + * + * Register and address definitions for AK8963 magnetometer. + */ + +#ifndef AK8963_REGS_H +#define AK8963_REGS_H + +#define AK8963_ADDR 0x1A + +/* Compass device dependent definition */ +#define AK8963_CNTL1_POWER_DOWN 0x10 +#define AK8963_CNTL1_SNG_MEASURE 0x11 +#define AK8963_CNTL1_CM_1 0x12 +#define AK8963_CNTL1_CM_2 0x16 +#define AK8963_CNTL1_EXT_TRIG 0x14 +#define AK8963_CNTL1_SELF_TEST 0x18 +#define AK8963_CNTL1_FUSE_ACCESS 0x1F + +/* AK8963 register address */ +#define AK8963_REG_WIA 0x00 +#define AK8963_REG_INFO 0x01 +#define AK8963_REG_ST1 0x02 +#define AK8963_REG_HXL 0x03 +#define AK8963_REG_HXH 0x04 +#define AK8963_REG_HYL 0x05 +#define AK8963_REG_HYH 0x06 +#define AK8963_REG_HZL 0x07 +#define AK8963_REG_HZH 0x08 +#define AK8963_REG_ST2 0x09 +#define AK8963_REG_CNTL1 0x0A +#define AK8963_REG_CNTL2 0x0B +#define AK8963_REG_ASTC 0x0C +#define AK8963_REG_TS1 0x0D +#define AK8963_REG_TS2 0x0E +#define AK8963_REG_I2CDIS 0x0F + +/* AK8963 fuse-rom address */ +#define AK8963_FUSE_ASAX 0x10 +#define AK8963_FUSE_ASAY 0x11 +#define AK8963_FUSE_ASAZ 0x12 + +#endif /* AK8963_REGS_H */ From 0d3e40a8a2b524e29ca09323c1dccd53963584eb Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 17 Nov 2014 20:53:24 +0100 Subject: [PATCH 30/86] [peripheral] fix some stuff in the ak8963 driver --- sw/airborne/peripherals/ak8963.c | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/sw/airborne/peripherals/ak8963.c b/sw/airborne/peripherals/ak8963.c index c9dd06ff7c5..f029394ec82 100644 --- a/sw/airborne/peripherals/ak8963.c +++ b/sw/airborne/peripherals/ak8963.c @@ -40,17 +40,13 @@ void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr) ak->i2c_trans.status = I2CTransDone; ak->initialized = FALSE; ak->init_status = AK_CONF_UNINIT; - - // Soft reset the device - ak->i2c_trans.buf[0] = AK8963_REG_CNTL2; - ak->i2c_trans.buf[1] = 1; - i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + ak->data_available = FALSE; } void ak8963_configure(struct Ak8963 *ak) { // Only configure when not busy - if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed) { + if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed && ak->i2c_trans.status != I2CTransDone) { return; } @@ -117,6 +113,7 @@ void ak8963_event(struct Ak8963 *ak) ak->data.vect.x = Int16FromBuf(ak->i2c_trans.buf, 0); ak->data.vect.y = Int16FromBuf(ak->i2c_trans.buf, 2); ak->data.vect.z = Int16FromBuf(ak->i2c_trans.buf, 4); + ak->data_available = TRUE; // Read second status register to be ready for reading again ak->i2c_trans.buf[0] = AK8963_REG_ST2; @@ -125,15 +122,21 @@ void ak8963_event(struct Ak8963 *ak) break; } - ak->i2c_trans.buf[0] = AK8963_REG_HXL; - i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 6); break; default: - // Goto idle - ak->data_available = TRUE; - ak->i2c_trans.status = I2CTransDone; - ak->status = AK_STATUS_IDLE; + if (ak->i2c_trans.status == I2CTransSuccess || ak->i2c_trans.status == I2CTransFailed) { + // Goto idle + ak->i2c_trans.status = I2CTransDone; + ak->status = AK_STATUS_IDLE; + // check overrun + //if (bit_is_set(ak->i2c_trans.buf[0], 3)) { + // ak->data_available = FALSE; + //} else { + // ak->data_available = TRUE; + //} + } break; } } + From 18cbec7f1550dc7664d7c618f9c9424fb13f8f1f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 17 Nov 2014 21:43:05 +0100 Subject: [PATCH 31/86] [peripheral] update mpu9250 driver, imu and module as well --- ...9250.makefile => imu_mpu9250_i2c.makefile} | 6 +- conf/modules/imu_mpu9250.xml | 1 + sw/airborne/modules/sensors/imu_mpu9250.c | 7 ++ sw/airborne/peripherals/mpu9250.c | 2 +- sw/airborne/peripherals/mpu9250.h | 4 +- sw/airborne/peripherals/mpu9250_i2c.c | 37 +++++++---- sw/airborne/peripherals/mpu9250_i2c.h | 2 + sw/airborne/peripherals/mpu9250_regs.h | 66 ++++++++++--------- .../imu/{imu_mpu9250.c => imu_mpu9250_i2c.c} | 40 +++++++---- .../imu/{imu_mpu9250.h => imu_mpu9250_i2c.h} | 8 +-- 10 files changed, 106 insertions(+), 67 deletions(-) rename conf/firmwares/subsystems/shared/{imu_mpu9250.makefile => imu_mpu9250_i2c.makefile} (87%) rename sw/airborne/subsystems/imu/{imu_mpu9250.c => imu_mpu9250_i2c.c} (80%) rename sw/airborne/subsystems/imu/{imu_mpu9250.h => imu_mpu9250_i2c.h} (95%) diff --git a/conf/firmwares/subsystems/shared/imu_mpu9250.makefile b/conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile similarity index 87% rename from conf/firmwares/subsystems/shared/imu_mpu9250.makefile rename to conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile index c7fe3ae54e4..e08d442631c 100644 --- a/conf/firmwares/subsystems/shared/imu_mpu9250.makefile +++ b/conf/firmwares/subsystems/shared/imu_mpu9250_i2c.makefile @@ -8,14 +8,14 @@ ifeq ($(TARGET), ap) IMU_MPU9250_CFLAGS = -DUSE_IMU endif -IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250.h\" +IMU_MPU9250_CFLAGS += -DIMU_TYPE_H=\"imu/imu_mpu9250_i2c.h\" IMU_MPU9250_SRCS = $(SRC_SUBSYSTEMS)/imu.c -IMU_MPU9250_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250.c +IMU_MPU9250_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_mpu9250_i2c.c IMU_MPU9250_SRCS += peripherals/mpu9250.c IMU_MPU9250_SRCS += peripherals/mpu9250_i2c.c # Magnetometer -#IMU_MPU9250_SRCS += peripherals/ak8963.c +IMU_MPU9250_SRCS += peripherals/ak8963.c # set default i2c bus diff --git a/conf/modules/imu_mpu9250.xml b/conf/modules/imu_mpu9250.xml index 5454c1eda95..c1b39854ed7 100644 --- a/conf/modules/imu_mpu9250.xml +++ b/conf/modules/imu_mpu9250.xml @@ -20,6 +20,7 @@ + diff --git a/sw/airborne/modules/sensors/imu_mpu9250.c b/sw/airborne/modules/sensors/imu_mpu9250.c index 5ec11265cf6..1d72446d4fe 100644 --- a/sw/airborne/modules/sensors/imu_mpu9250.c +++ b/sw/airborne/modules/sensors/imu_mpu9250.c @@ -67,5 +67,12 @@ void imu_mpu9250_report(void) (int32_t)(mpu9250.data_rates.rates.r) }; DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &rates.p, &rates.q, &rates.r); + + struct Int32Vect3 mag = { + (int32_t)(mpu9250.akm.data.vect.x), + (int32_t)(mpu9250.akm.data.vect.y), + (int32_t)(mpu9250.akm.data.vect.z) + }; + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag.x, &mag.y, &mag.z); } diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index ebdb23cf88f..9a3df5c7793 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -81,7 +81,7 @@ void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Conf break; case MPU9250_CONF_DLPF_ACCEL: /* configure digital low pass filter fir accelerometer */ - mpu_set(mpu, MPU9250_REG_CONFIG, config->dlpf_accel_cfg); + mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG_2, config->dlpf_accel_cfg); config->init_status++; break; case MPU9250_CONF_GYRO: diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 2fcfed58bf2..70330b3e810 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -35,9 +35,9 @@ /// Default sample rate divider #define MPU9250_DEFAULT_SMPLRT_DIV 0 /// Default gyro full scale range +- 2000°/s -#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_2000 +#define MPU9250_DEFAULT_FS_SEL MPU9250_GYRO_RANGE_1000 /// Default accel full scale range +- 16g -#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_16G +#define MPU9250_DEFAULT_AFS_SEL MPU9250_ACCEL_RANGE_8G /// Default internal sampling (1kHz, 42Hz LP Bandwidth) #define MPU9250_DEFAULT_DLPF_ACCEL_CFG MPU9250_DLPF_ACCEL_41HZ /// Default internal sampling (1kHz, 42Hz LP Bandwidth) diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index b995e12003c..3237c35224b 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -27,6 +27,8 @@ #include "peripherals/mpu9250_i2c.h" +bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))); + void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr) { /* set i2c_peripheral */ @@ -44,13 +46,16 @@ void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t mpu->config.initialized = FALSE; mpu->config.init_status = MPU9250_CONF_UNINIT; + /* "internal" ak8963 magnetometer */ + ak8963_init(&mpu->akm, i2c_p, MPU9250_MAG_ADDR); /* mag is declared as slave to call the configure function, * regardless if it is an actual MPU slave or passthrough */ - //mpu->config.nb_slaves = 1; + mpu->config.nb_slaves = 1; /* set callback function to configure mag */ - //mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave; - + mpu->config.slaves[0].configure = &imu_mpu9250_configure_mag_slave; + /* read the mag directly for now */ + mpu->config.i2c_bypass = TRUE; mpu->slave_init_status = MPU9250_I2C_CONF_UNINIT; } @@ -80,6 +85,12 @@ void mpu9250_i2c_read(struct Mpu9250_I2c *mpu) /* set read bit and multiple byte bit, then address */ mpu->i2c_trans.buf[0] = MPU9250_REG_INT_STATUS; i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes); + /* read mag */ +#ifdef MPU9250_MAG_PRESCALER + RunOnceEvery(MPU9250_MAG_PRESCALER, ak8963_read(&mpu->akm)); +#else + ak8963_read(&mpu->akm); +#endif } } @@ -125,19 +136,23 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) break; } } + // Ak8963 event function + ak8963_event(&mpu->akm); } /** callback function to configure ak8963 mag * @return TRUE if mag configuration finished */ -//bool_t imu_mpu9250_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) -//{ -// ak8963_start_configure(&imu_mpu9250.akm); -// if (imu_mpu9250.akm.initialized) -// return TRUE; -// else -// return FALSE; -//} +bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu) +{ + struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); + + ak8963_configure(&mpu_i2c->akm); + if (mpu_i2c->akm.initialized) + return TRUE; + else + return FALSE; +} /** @todo: only one slave so far. */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) diff --git a/sw/airborne/peripherals/mpu9250_i2c.h b/sw/airborne/peripherals/mpu9250_i2c.h index 3c1e4842f4e..a826e818e1a 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.h +++ b/sw/airborne/peripherals/mpu9250_i2c.h @@ -34,6 +34,7 @@ /* Include common MPU9250 options and definitions */ #include "peripherals/mpu9250.h" +#include "peripherals/ak8963.h" #define MPU9250_BUFFER_EXT_LEN 16 @@ -66,6 +67,7 @@ struct Mpu9250_I2c { uint8_t data_ext[MPU9250_BUFFER_EXT_LEN]; struct Mpu9250Config config; enum Mpu9250I2cSlaveInitStatus slave_init_status; + struct Ak8963 akm; ///< "internal" magnetometer }; // Functions diff --git a/sw/airborne/peripherals/mpu9250_regs.h b/sw/airborne/peripherals/mpu9250_regs.h index ab452fd18e2..d680e8ecf4b 100644 --- a/sw/airborne/peripherals/mpu9250_regs.h +++ b/sw/airborne/peripherals/mpu9250_regs.h @@ -32,6 +32,8 @@ #define MPU9250_ADDR 0xD0 #define MPU9250_ADDR_ALT 0xD2 +#define MPU9250_MAG_ADDR 0x18 + #define MPU9250_SPI_READ 0x80 // Power and Interface @@ -43,15 +45,15 @@ // FIFO #define MPU9250_REG_FIFO_EN 0x23 #define MPU9250_REG_FIFO_COUNT_H 0x72 -#define MPU9250_REG_FIFO_COUNT_L 0x73 -#define MPU9250_REG_FIFO_R_W 0x74 +#define MPU9250_REG_FIFO_COUNT_L 0x73 +#define MPU9250_REG_FIFO_R_W 0x74 // Measurement Settings -#define MPU9250_REG_SMPLRT_DIV 0x19 -#define MPU9250_REG_CONFIG 0x1A -#define MPU9250_REG_GYRO_CONFIG 0x1B -#define MPU9250_REG_ACCEL_CONFIG 0x1C -#define MPU9250_REG_ACCEL_CONFIG_2 0x1D +#define MPU9250_REG_SMPLRT_DIV 0x19 +#define MPU9250_REG_CONFIG 0x1A +#define MPU9250_REG_GYRO_CONFIG 0x1B +#define MPU9250_REG_ACCEL_CONFIG 0x1C +#define MPU9250_REG_ACCEL_CONFIG_2 0x1D // I2C Slave settings @@ -59,31 +61,31 @@ #define MPU9250_REG_I2C_MST_STATUS 0x36 #define MPU9250_REG_I2C_MST_DELAY 0x67 // Slave 0 -#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr -#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg -#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits -#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO +#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr +#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg +#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits +#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO // Slave 1 -#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr -#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg -#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits -#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO +#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr +#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg +#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits +#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO // Slave 2 -#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr -#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg -#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits -#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO +#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr +#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg +#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits +#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO // Slave 3 -#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr -#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg -#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits -#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO +#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr +#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg +#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits +#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO // Slave 4 - special -#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr -#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg -#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO -#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits -#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI +#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr +#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg +#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO +#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits +#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI // Interrupt #define MPU9250_REG_INT_PIN_CFG 0x37 @@ -116,7 +118,7 @@ #define MPU9250_REG_WHO_AM_I 0x75 -#define MPU9250_WHOAMI_REPLY 0x68 +#define MPU9250_WHOAMI_REPLY 0x68 // Bit positions #define MPU9250_I2C_BYPASS_EN 1 @@ -126,7 +128,7 @@ #define MPU9250_I2C_MST_RESET 1 #define MPU9250_FIFO_RESET 2 #define MPU9250_I2C_IF_DIS 4 -#define MPU9250_I2C_MST_EN 5 +#define MPU9250_I2C_MST_EN 5 #define MPU9250_FIFO_EN 6 // in MPU9250_REG_I2C_MST_STATUS @@ -135,7 +137,7 @@ /** Digital Low Pass Filter Options */ enum Mpu9250DLPFGyro { - MPU9250_DLPF__GYRO_250HZ = 0x0, // internal sampling rate 8kHz + MPU9250_DLPF_GYRO_250HZ = 0x0, // internal sampling rate 8kHz MPU9250_DLPF_GYRO_184HZ = 0x1, // internal sampling rate 1kHz MPU9250_DLPF_GYRO_92HZ = 0x2, MPU9250_DLPF_GYRO_41HZ = 0x3, @@ -144,7 +146,6 @@ enum Mpu9250DLPFGyro { MPU9250_DLPF_GYRO_05HZ = 0x6 }; - enum Mpu9250DLPFAccel { MPU9250_DLPF_ACCEL_460HZ = 0x0, // internal sampling rate 8kHz MPU9250_DLPF_ACCEL_184HZ = 0x1, // internal sampling rate 1kHz @@ -154,6 +155,7 @@ enum Mpu9250DLPFAccel { MPU9250_DLPF_ACCEL_10HZ = 0x5, MPU9250_DLPF_ACCEL_05HZ = 0x6 }; + /** * Selectable gyro range */ diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.c b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c similarity index 80% rename from sw/airborne/subsystems/imu/imu_mpu9250.c rename to sw/airborne/subsystems/imu/imu_mpu9250_i2c.c index ea9f6179fe0..24a49708c8d 100644 --- a/sw/airborne/subsystems/imu/imu_mpu9250.c +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c @@ -19,7 +19,7 @@ */ /** - * @file subsystems/imu/imu_mpu9250.c + * @file subsystems/imu/imu_mpu9250_i2c.c * * IMU driver for the MPU9250 using I2C * @@ -31,21 +31,29 @@ #if !defined IMU_MPU9250_GYRO_LOWPASS_FILTER && !defined IMU_MPU9250_ACCEL_LOWPASS_FILTER && !defined IMU_MPU9250_SMPLRT_DIV #if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) -/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms - * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz +/* Accelerometer: Bandwidth 41Hz, Delay 5.9ms + * Gyroscope: Bandwidth 41Hz, Delay 5.9ms sampling 1kHz + * Output rate: 100Hz */ #define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ #define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ #define IMU_MPU9250_SMPLRT_DIV 9 PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") #elif PERIODIC_FREQUENCY == 512 -/* Accelerometer: Bandwidth 260Hz, Delay 0ms - * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz +/* Accelerometer: Bandwidth 184Hz, Delay 5.8ms + * Gyroscope: Bandwidth 250Hz, Delay 0.97ms sampling 8kHz + * Output rate: 2kHz */ -#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ -#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ +#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_250HZ +#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_184HZ #define IMU_MPU9250_SMPLRT_DIV 3 PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling") +#else +/* By default, don't go too fast */ +#define IMU_MPU9250_SMPLRT_DIV 9 +#define IMU_MPU9250_GYRO_LOWPASS_FILTER MPU9250_DLPF_GYRO_41HZ +#define IMU_MPU9250_ACCEL_LOWPASS_FILTER MPU9250_DLPF_ACCEL_41HZ +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") #endif #endif PRINT_CONFIG_VAR(IMU_MPU9250_SMPLRT_DIV) @@ -122,20 +130,24 @@ void imu_mpu9250_event(void) (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Y]), (int32_t)(imu_mpu9250.mpu.data_rates.value[IMU_MPU9250_CHAN_Z]) }; - //struct Int32Vect3 mag = { - // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_X]), - // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Y]), - // (int32_t)(imu_mpu9250.mpu.data_mag.value[IMU_MPU9250_CHAN_Z]) - //}; // unscaled vector VECT3_COPY(imu.accel_unscaled, accel); RATES_COPY(imu.gyro_unscaled, rates); - //VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mpu.data_available = FALSE; imu_mpu9250.gyro_valid = TRUE; imu_mpu9250.accel_valid = TRUE; - //imu_mpu9250.mag_valid = TRUE; + } + // Test if mag data are updated + if (imu_mpu9250.mpu.akm.data_available) { + struct Int32Vect3 mag = { + (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), + (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), + (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) + }; + VECT3_COPY(imu.mag_unscaled, mag); + imu_mpu9250.mag_valid = TRUE; + imu_mpu9250.mpu.akm.data_available = FALSE; } } diff --git a/sw/airborne/subsystems/imu/imu_mpu9250.h b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h similarity index 95% rename from sw/airborne/subsystems/imu/imu_mpu9250.h rename to sw/airborne/subsystems/imu/imu_mpu9250_i2c.h index 42d0b8eff89..2a39b804f7a 100644 --- a/sw/airborne/subsystems/imu/imu_mpu9250.h +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h @@ -19,14 +19,14 @@ */ /** - * @file subsystems/imu/imu_mpu9250.h + * @file subsystems/imu/imu_mpu9250_i2c.h * * IMU driver for the MPU9250 using I2C * */ -#ifndef IMU_MPU9250_H -#define IMU_MPU9250_H +#ifndef IMU_MPU9250_I2C_H +#define IMU_MPU9250_I2C_H #include "std.h" #include "generated/airframe.h" @@ -98,4 +98,4 @@ static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler } } -#endif /* IMU_MPU9250_H */ +#endif /* IMU_MPU9250_I2C_H */ From fadb06771f582db22eec14f09f0e6a9fe44cac71 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 13 Nov 2014 17:31:09 +0100 Subject: [PATCH 32/86] [peripherals] mpu60x0: use gcc pragma to ignore cast-qual warnings The buffer is volatile, since filled from ISR... But we know it's ok to use it here so we silence the warning. Also there is a bug in some gcc version that incorrectly reports this as peripherals/mpu60x0_spi.c:118:33: warning: cast discards '__attribute__((noreturn))' qualifier from pointer target type [-Wcast-qual] see https://gcc.gnu.org/bugzilla/show_bug.cgi?id=55383 --- sw/airborne/peripherals/mpu60x0_i2c.c | 11 +++++++++-- sw/airborne/peripherals/mpu60x0_spi.c | 11 +++++++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c index 2c79d814a33..29aa3bea649 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.c +++ b/sw/airborne/peripherals/mpu60x0_i2c.c @@ -96,8 +96,15 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13); // if we are reading slaves through the mpu, copy the ext_sens_data - if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) - memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) { + /* the buffer is volatile, since filled from ISR + * but we know it's ok to use it here so we silence the warning + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + memcpy(mpu->data_ext, (uint8_t*)&(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); +#pragma GCC diagnostic pop + } mpu->data_available = TRUE; } diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index 0689e972ad5..8f166f9ab78 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -114,8 +114,15 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14); // if we are reading slaves, copy the ext_sens_data - if (mpu->config.nb_slaves > 0) - memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + if (mpu->config.nb_slaves > 0) { + /* the buffer is volatile, since filled from ISR + * but we know it's ok to use it here so we silence the warning + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + memcpy(mpu->data_ext, (uint8_t*)&(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); +#pragma GCC diagnostic pop + } mpu->data_available = TRUE; } From b17322ecd58151381dda063f76d10528ee2efbed Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 18 Nov 2014 10:05:46 +0100 Subject: [PATCH 33/86] [peripherals] ignore cast-qual for mpu9250 as well --- sw/airborne/peripherals/mpu9250_i2c.c | 11 +++++++++-- sw/airborne/peripherals/mpu9250_spi.c | 11 +++++++++-- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index 3237c35224b..25ae8e298b3 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -114,8 +114,15 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13); // if we are reading slaves through the mpu, copy the ext_sens_data - if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) - memcpy(mpu->data_ext, (void *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) { + /* the buffer is volatile, since filled from ISR + * but we know it's ok to use it here so we silence the warning + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + memcpy(mpu->data_ext, (uint8_t *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); +#pragma GCC diagnostic pop + } mpu->data_available = TRUE; } diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c index 0fff4d53305..82f03067e1b 100644 --- a/sw/airborne/peripherals/mpu9250_spi.c +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -114,8 +114,15 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14); // if we are reading slaves, copy the ext_sens_data - if (mpu->config.nb_slaves > 0) - memcpy(mpu->data_ext, (void *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + if (mpu->config.nb_slaves > 0) { + /* the buffer is volatile, since filled from ISR + * but we know it's ok to use it here so we silence the warning + */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + memcpy(mpu->data_ext, (uint8_t *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); +#pragma GCC diagnostic pop + } mpu->data_available = TRUE; } From ec865d84f04f15b2815e4b30665326e05f3f9b73 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 18 Nov 2014 14:08:28 +0100 Subject: [PATCH 34/86] [imu] mpu9250 mag has a different channel order than gyro/accel --- sw/airborne/subsystems/imu/imu_mpu9250_i2c.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c index 24a49708c8d..e96e8206d85 100644 --- a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.c @@ -141,9 +141,9 @@ void imu_mpu9250_event(void) // Test if mag data are updated if (imu_mpu9250.mpu.akm.data_available) { struct Int32Vect3 mag = { - (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Y]), - (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) + (int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_X]), + -(int32_t)(imu_mpu9250.mpu.akm.data.value[IMU_MPU9250_CHAN_Z]) }; VECT3_COPY(imu.mag_unscaled, mag); imu_mpu9250.mag_valid = TRUE; From dcea7274c5a368f0e806b3a76391696b698b6c17 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 14:23:58 +0100 Subject: [PATCH 35/86] [airborne] don't call uart_event() in simulators uart_event was added as a weak function with #951, but it is not provided in simulators. The compiler (correctly) fills in a zero and the simulator segfaults. For now simply fix it by not calling uart_event in simulators. --- sw/airborne/firmwares/fixedwing/main_fbw.c | 2 ++ sw/airborne/firmwares/rotorcraft/main.c | 2 ++ 2 files changed, 4 insertions(+) diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index edc520c8d7c..2f15979bbb7 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -189,7 +189,9 @@ void event_task_fbw( void) { i2c_event(); #endif +#ifndef SITL uart_event(); +#endif #ifdef INTER_MCU #if defined MCU_SPI_LINK | defined MCU_UART_LINK diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index d90ec9d50cb..d1cd09b2153 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -275,7 +275,9 @@ STATIC_INLINE void main_event( void ) { i2c_event(); +#ifndef SITL uart_event(); +#endif #if USE_UDP udp_event(); From 80953a84989dc7ae444613b39e36a44925bb8696 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 15:18:42 +0100 Subject: [PATCH 36/86] [stm32] replace remaining rcc_peripheral_enable_clock by less error prone rcc_periph_clock_enable --- sw/airborne/arch/stm32/led_hw.h | 5 ++--- sw/airborne/arch/stm32/uart_tunnel.c | 5 +---- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/sw/airborne/arch/stm32/led_hw.h b/sw/airborne/arch/stm32/led_hw.h index 8d2a5108399..d5004de98c4 100644 --- a/sw/airborne/arch/stm32/led_hw.h +++ b/sw/airborne/arch/stm32/led_hw.h @@ -71,9 +71,8 @@ extern uint8_t led_status[NB_LED]; /* PC15 led_data */ #define LED_INIT(_i) { \ - rcc_peripheral_enable_clock(&RCC_APB2ENR, \ - RCC_APB2ENR_IOPAEN | \ - RCC_APB2ENR_IOPCEN); \ + rcc_periph_clock_enable(RCC_GPIOA); \ + rcc_periph_clock_enable(RCC_GPIOC); \ gpio_set_mode(GPIOA, \ GPIO_MODE_OUTPUT_50_MHZ, \ GPIO_CNF_OUTPUT_PUSHPULL, \ diff --git a/sw/airborne/arch/stm32/uart_tunnel.c b/sw/airborne/arch/stm32/uart_tunnel.c index 948a562a3f6..e0151b3b56f 100644 --- a/sw/airborne/arch/stm32/uart_tunnel.c +++ b/sw/airborne/arch/stm32/uart_tunnel.c @@ -28,7 +28,6 @@ #include "led.h" /* UART1 */ -#define A_PERIPH RCC_APB2ENR_IOPAEN #define A_PORT GPIOA #define A_RX_PIN GPIO10 #define A_RX_PORT A_PORT @@ -36,7 +35,6 @@ #define A_TX_PORT A_PORT /* UART2 */ -#define B_PERIPH RCC_APB2ENR_IOPAEN #define B_PORT GPIOA #define B_RX_PIN GPIO3 #define B_RX_PORT B_PORT @@ -57,8 +55,7 @@ int main(void) { sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); /* init RCC */ - rcc_peripheral_enable_clock(&RCC_APB2ENR, A_PERIPH); - // rccp_perihperal_enable_clock(&RCC_APB2ENR, B_PERIPH); + rcc_periph_clock_enable(RCC_GPIOA); /* Init GPIO for rx pins */ gpio_set(A_RX_PORT, A_RX_PIN); From 82d3015801382a2b91b95258e83f989d1e26f467 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 18 Nov 2014 16:55:51 +0100 Subject: [PATCH 37/86] [light] allow to define a led for light module in airframe.h, not only as compile flag --- conf/airframes/ENAC/fixed-wing/twinjet2.xml | 60 +++++--- sw/airborne/modules/light/light.c | 151 ++++++++++---------- sw/airborne/modules/light/light.h | 4 - 3 files changed, 115 insertions(+), 100 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/twinjet2.xml b/conf/airframes/ENAC/fixed-wing/twinjet2.xml index e11d38f3e7b..91994e86718 100644 --- a/conf/airframes/ENAC/fixed-wing/twinjet2.xml +++ b/conf/airframes/ENAC/fixed-wing/twinjet2.xml @@ -2,30 +2,17 @@ - + - - - - - - - - - @@ -33,7 +20,8 @@ - + + @@ -41,7 +29,6 @@ - @@ -78,8 +65,8 @@ - - + +
@@ -87,7 +74,31 @@
-
+
+ + + + + + + + + + + + + + + + + + + + + +
+ +
@@ -121,8 +132,11 @@ - - + + + + +
diff --git a/sw/airborne/modules/light/light.c b/sw/airborne/modules/light/light.c index 9a2a94a5003..9f4136f76f8 100644 --- a/sw/airborne/modules/light/light.c +++ b/sw/airborne/modules/light/light.c @@ -20,12 +20,17 @@ * */ -#include "light.h" +#include "modules/light/light.h" #include "led.h" +#include "generated/airframe.h" uint8_t strobe_light_mode; uint8_t nav_light_mode; +#ifndef LIGHT_LED_STROBE +#define LIGHT_LED_STROBE 2 +#endif + #ifndef STROBE_LIGHT_MODE_DEFAULT #define STROBE_LIGHT_MODE_DEFAULT 6 #endif @@ -57,92 +62,92 @@ void periodic_light(void) switch (strobe_light_mode) { default: // Always off - LED_OFF(LIGHT_LED_STROBE); - break; + LED_OFF(LIGHT_LED_STROBE); + break; case 1: // Always on - LED_ON(LIGHT_LED_STROBE); - break; + LED_ON(LIGHT_LED_STROBE); + break; case 2: // Blink case 3: case 4: - if (counter == (strobe_light_mode*5 - 4)) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 20) - { - LED_ON(LIGHT_LED_STROBE); - counter = 0; - } - break; - case 5: // Complex Blinking - if (counter == 3) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 4) - { - LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 6) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 7) - { - LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 8) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 25) - { - LED_ON(LIGHT_LED_STROBE); - counter = 0; - } - break; - case 6: - if (counter <= 18) - { - if ((counter % 2) == 0) - { - LED_ON(LIGHT_LED_STROBE); - } - else - { - LED_OFF(LIGHT_LED_STROBE); - } - } - else if (counter == 35) - { - counter = 0; - } - break; + if (counter == (strobe_light_mode*5 - 4)) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter >= 20) + { + LED_ON(LIGHT_LED_STROBE); + counter = 0; + } + break; + case 5: // Complex Blinking + if (counter == 3) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter == 4) + { + LED_ON(LIGHT_LED_STROBE); + } + else if (counter == 6) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter == 7) + { + LED_ON(LIGHT_LED_STROBE); + } + else if (counter == 8) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter >= 25) + { + LED_ON(LIGHT_LED_STROBE); + counter = 0; + } + break; + case 6: + if (counter <= 18) + { + if ((counter % 2) == 0) + { + LED_ON(LIGHT_LED_STROBE); + } + else + { + LED_OFF(LIGHT_LED_STROBE); + } + } + else if (counter == 35) + { + counter = 0; + } + break; } #ifdef LIGHT_LED_NAV switch (nav_light_mode) { default: // Always off - LED_OFF(LIGHT_LED_NAV); - break; + LED_OFF(LIGHT_LED_NAV); + break; case 1: // Always on - LED_ON(LIGHT_LED_NAV); - break; + LED_ON(LIGHT_LED_NAV); + break; case 2: // Blink case 3: case 4: - if (counter_nav == (nav_light_mode*5 - 4)) - { - LED_OFF(LIGHT_LED_NAV); - } - else if (counter_nav >= 20) - { - LED_ON(LIGHT_LED_NAV); - counter_nav = 0; - } - break; + if (counter_nav == (nav_light_mode*5 - 4)) + { + LED_OFF(LIGHT_LED_NAV); + } + else if (counter_nav >= 20) + { + LED_ON(LIGHT_LED_NAV); + counter_nav = 0; + } + break; } counter_nav++; #endif diff --git a/sw/airborne/modules/light/light.h b/sw/airborne/modules/light/light.h index 0216efe8258..9ab1f6310e1 100644 --- a/sw/airborne/modules/light/light.h +++ b/sw/airborne/modules/light/light.h @@ -30,10 +30,6 @@ #include -#ifndef LIGHT_LED_STROBE -#define LIGHT_LED_STROBE 2 -#endif - extern uint8_t strobe_light_mode; extern uint8_t nav_light_mode; From 68e907947e2edf1aa3b581015860a57cb8037705 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 16:55:11 +0100 Subject: [PATCH 38/86] [tests] get number of test targets for plan --- tests/examples/01_compile_all_aircrafts.t | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tests/examples/01_compile_all_aircrafts.t b/tests/examples/01_compile_all_aircrafts.t index d535711af1d..2e58fcfe17b 100644 --- a/tests/examples/01_compile_all_aircrafts.t +++ b/tests/examples/01_compile_all_aircrafts.t @@ -37,6 +37,26 @@ $|++; my $xmlSimple = XML::Simple->new(ForceArray => 1); my $conf = $xmlSimple->XMLin("$ENV{'PAPARAZZI_HOME'}/conf/conf.xml"); + +sub get_num_targets +{ + my $num_targets = 0; + foreach my $aircraft (sort keys%{$conf->{'aircraft'}}) + { + my $airframe = $conf->{'aircraft'}->{$aircraft}->{'airframe'}; + my $airframe_config = $xmlSimple->XMLin("$ENV{'PAPARAZZI_HOME'}/conf/$airframe"); + foreach my $process (sort keys %{$airframe_config->{'firmware'}}) + { + foreach my $target (sort keys %{$airframe_config->{'firmware'}->{$process}->{'target'}}) + { + $num_targets++; + } + } + } + return $num_targets; +} +plan tests => get_num_targets()+1; + ok(1, "Parsed the configuration file"); foreach my $aircraft (sort keys%{$conf->{'aircraft'}}) { From c03427b7003ad7a02a445af0b3f6d6ca9417d959 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 14:49:28 +0100 Subject: [PATCH 39/86] [math] int32_sqrt returns unsigned int and takes unsigned int as arg --- sw/airborne/math/pprz_algebra_int.c | 10 +++++----- sw/airborne/math/pprz_algebra_int.h | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c index 1289a1cc71c..abe8830defc 100644 --- a/sw/airborne/math/pprz_algebra_int.c +++ b/sw/airborne/math/pprz_algebra_int.c @@ -27,7 +27,7 @@ #include "pprz_algebra_int.h" #define INT32_SQRT_MAX_ITER 40 -int32_t int32_sqrt(int32_t in) +uint32_t int32_sqrt(uint32_t in) { if (in == 0) { return 0; @@ -407,7 +407,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) const int32_t tr = RMAT_TRACE(*r); if (tr > 0) { const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; - int32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC); + uint32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC); two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); q->qi = two_qi / 2; q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << @@ -424,7 +424,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 2, 2)) { const int32_t two_qx_two = RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 1, 1) - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.); - int32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC); + uint32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC); two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) @@ -439,7 +439,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) } else if (RMAT_ELMT(*r, 1, 1) > RMAT_ELMT(*r, 2, 2)) { const int32_t two_qy_two = RMAT_ELMT(*r, 1, 1) - RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.); - int32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC); + uint32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC); two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) @@ -454,7 +454,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) } else { const int32_t two_qz_two = RMAT_ELMT(*r, 2, 2) - RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 1, 1) + TRIG_BFP_OF_REAL(1.); - int32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC); + uint32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC); two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 837ac2b73ef..00c75f06212 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -221,7 +221,7 @@ struct Int64Vect3 { #define INT_MULT_RSHIFT(_a, _b, _r) (((_a)*(_b))>>(_r)) -extern int32_t int32_sqrt(int32_t in); +extern uint32_t int32_sqrt(uint32_t in); #define INT32_SQRT(_out,_in) { _out = int32_sqrt(_in); } @@ -433,9 +433,9 @@ static inline void int32_quat_identity(struct Int32Quat* q) /** Norm of a quaternion. */ -static inline int32_t int32_quat_norm(struct Int32Quat* q) +static inline uint32_t int32_quat_norm(struct Int32Quat* q) { - int32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz; + uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz; return int32_sqrt(n2); } From b0070b261d131f3529cfc9e73c6e5c6a36ae9d7f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 14:55:54 +0100 Subject: [PATCH 40/86] [state interface] speed_norm_i is unsigned --- sw/airborne/state.c | 12 ++++++------ sw/airborne/state.h | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/sw/airborne/state.c b/sw/airborne/state.c index 9a32002cb69..cb80491c986 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -762,8 +762,8 @@ void stateCalcHorizontalSpeedNorm_i(void) { state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { - int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + - state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + + state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { @@ -772,8 +772,8 @@ void stateCalcHorizontalSpeedNorm_i(void) { state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { - int32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x + - state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC; + uint32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x + + state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { @@ -785,8 +785,8 @@ void stateCalcHorizontalSpeedNorm_i(void) { /* transform ecef speed to ned, set status bit, then compute norm */ ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); SetBit(state.speed_status, SPEED_NED_I); - int32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + - state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + + state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { diff --git a/sw/airborne/state.h b/sw/airborne/state.h index faea93c479c..f6c1273f424 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -277,7 +277,7 @@ struct State { * Norm of horizontal ground speed. * Unit: m/s in BFP with #INT32_SPEED_FRAC */ - int32_t h_speed_norm_i; + uint32_t h_speed_norm_i; /** * Direction of horizontal ground speed. @@ -816,7 +816,7 @@ static inline struct EcefCoor_i* stateGetSpeedEcef_i(void) { } /// Get norm of horizontal ground speed (int). -static inline int32_t* stateGetHorizontalSpeedNorm_i(void) { +static inline uint32_t* stateGetHorizontalSpeedNorm_i(void) { if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) stateCalcHorizontalSpeedNorm_i(); return &state.h_speed_norm_i; From dd0c6cc1faa93b4132627334650b3243102b7073 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 15:46:10 +0100 Subject: [PATCH 41/86] [rotorcraft] navigation: unsigned nav_leg_length --- sw/airborne/firmwares/rotorcraft/navigation.c | 6 +++--- sw/airborne/firmwares/rotorcraft/navigation.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index b99afe568a8..2dee31333d4 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -77,7 +77,7 @@ struct EnuCoor_i nav_circle_center; int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians; int32_t nav_leg_progress; -int32_t nav_leg_length; +uint32_t nav_leg_length; int32_t nav_roll, nav_pitch; int32_t nav_heading; @@ -269,8 +269,8 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) { VECT2_COPY(wp_diff_prec, wp_diff); INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC); INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC); - int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1); - INT32_SQRT(nav_leg_length,leg_length2); + uint32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1); + nav_leg_length = int32_sqrt(leg_length2); nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y) / nav_leg_length; int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0); nav_leg_progress += progress; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 4aa00f5906f..89f74dffa06 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -60,7 +60,7 @@ extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC extern float nav_radius; extern int32_t nav_leg_progress; -extern int32_t nav_leg_length; +extern uint32_t nav_leg_length; extern uint8_t vertical_mode; extern uint32_t nav_throttle; ///< direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL From a99d6662d6220942a125c0d64bac47cd5a122782 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 17:37:00 +0100 Subject: [PATCH 42/86] [math] fix int32_vect2_normalize keep correct sign of vector --- sw/airborne/math/pprz_algebra_int.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index 00c75f06212..f24c55c1bc1 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -249,11 +249,11 @@ static inline uint32_t int32_vect2_norm(struct Int32Vect2* v) /** normalize 2D vector inplace */ static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac) { - const uint32_t f = BFP_OF_REAL((1.), frac); const uint32_t n = int32_vect2_norm(v); if (n > 0) { - v->x = v->x * f / n; - v->y = v->y * f / n; + const int32_t f = BFP_OF_REAL((1.), frac); + v->x = v->x * f / (int32_t)n; + v->y = v->y * f / (int32_t)n; } } From 9aee8b3777a1b0e112f84b95e49485ef3a7227f0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Nov 2014 10:53:23 +0100 Subject: [PATCH 43/86] create_module helper: only one init function --- create_module | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/create_module b/create_module index b7417ea9053..f754f69536b 100755 --- a/create_module +++ b/create_module @@ -50,9 +50,13 @@ let add_to_list = fun l a -> l := !l @ [a];; let ask_init = fun () -> - printf "Initialization function to call, eg \"foo_init()\":\n"; - let name = ask " - function (mandatory)" in - add_to_list init_list name; + if (List.length !init_list) > 0 then + printf "You already added an init function.\n" + else begin + printf "Initialization function to call, eg \"foo_init()\":\n"; + let name = ask_param "function" Mandatory in + add_to_list init_list name + end; true;; let ask_periodic = fun () -> From cbb497ed66e98baa47ad33594845697a4a3b892b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Nov 2014 15:39:46 +0100 Subject: [PATCH 44/86] [tests] show warnings by default --- .travis.yml | 2 +- tests/examples/01_compile_all_aircrafts.t | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.travis.yml b/.travis.yml index 741d22c15da..5c1ac90784f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,7 +11,7 @@ install: before_script: cd conf && ln -s conf_tests.xml conf.xml && cd .. script: - make - - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD SHOW_WARNINGS_ONLY=1 J=AUTO prove tests/examples/ + - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD J=AUTO prove tests/examples/ notifications: webhooks: diff --git a/tests/examples/01_compile_all_aircrafts.t b/tests/examples/01_compile_all_aircrafts.t index 2e58fcfe17b..3a0fb5764c2 100644 --- a/tests/examples/01_compile_all_aircrafts.t +++ b/tests/examples/01_compile_all_aircrafts.t @@ -10,17 +10,17 @@ # # optional environment variables: # TEST_VERBOSE : set to 1 to print the compile output even if there was no error -# SHOW_WARNINGS : set to 1 to print the complete compile output if there were warnings -# SHOW_WARNINGS_ONLY : set to 1 to print only the warnings +# SHOW_WARNINGS_FULL : set to 1 to print the complete compile output if there were warnings +# HIDE_WARNINGS : set to 1 to disable printing of warnings # # environment variables passed on to make: # J=AUTO : detect number of CPUs to set jobs for parallel compilation # # Examples on how to test compile all aircrafts/targets in your current conf.xml: +# only showing full compile output if there has been an error, if there were warnings only print those +# prove test/examples # with parallel compilation and showing full output during compilation # J=AUTO prove tests/examples -v -# only showing full compile output if there has been an error, if there were warnings only print those -# SHOW_WARNINGS_ONLY=1 prove test/examples # with parallel compilation and treating all warnings as errors: # J=AUTO USER_CFLAGS=-Werror prove tests/examples # @@ -81,17 +81,17 @@ foreach my $aircraft (sort keys%{$conf->{'aircraft'}}) warn "$output\n"; } # if successful, still print warnings if requested - elsif ($warnings && ($ENV{'SHOW_WARNINGS'} || $ENV{'SHOW_WARNINGS_ONLY'})) { + elsif ($warnings && !$ENV{'HIDE_WARNINGS'}) { if (!$ENV{'TEST_VERBOSE'}) { warn "\nWarning: AIRCRAFT=$aircraft target=$target compiled sucessfully but had warnings:\n"; - if ($ENV{'SHOW_WARNINGS_ONLY'}) { - warn "$warnings\n"; + if ($ENV{'SHOW_WARNINGS_FULL'}) { + warn "$output\n"; } else { - warn "$output\n"; + warn "$warnings\n"; } } - if (!$ENV{'SHOW_WARNINGS_ONLY'}) { + if ($ENV{'SHOW_WARNINGS_FULL'}) { warn "\nAIRCRAFT=$aircraft target=$target compiled sucessfully but had warnings.\n\n"; } } From 25fb27495446006ab9ac865be0f1065cb90fd1b4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Nov 2014 17:51:39 +0100 Subject: [PATCH 45/86] [simulator] OCaml sim: simulate sys_time should fix #962 --- sw/airborne/arch/sim/sim_ap.c | 14 ++++++++++++++ sw/simulator/sitl.ml | 3 +++ 2 files changed, 17 insertions(+) diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index dbc4dd7e281..51502e56179 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -47,6 +47,20 @@ uint16_t datalink_time = 0; uint8_t ac_id; +#if PERIODIC_FREQUENCY != 60 +#warning "Simple OCaml sim can currently only handle a PERIODIC_FREQUENCY of 60Hz" +#endif + +#if SYS_TIME_FREQUENCY != 120 +#warning "Simple OCaml sim can currently only handle a SYS_TIME_FREQUENCY of 120Hz" +#endif + +/** needs to be called at SYS_TIME_FREQUENCY */ +value sim_sys_time_task(value unit) { + sys_tick_handler(); + return unit; +} + value sim_periodic_task(value unit) { sensors_task(); attitude_loop(); diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index a889b820a65..25812139998 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -44,6 +44,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct let nav_period = 1./.4. (* s *) let monitor_period = 1. (* s *) let rc_period = 1./.40. (* s *) + let sys_time_period = 1./.120. (* s *) let msg = fun name -> ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message" @@ -125,6 +126,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct Stdlib.timer rc_period send_ppm; (** FIXME: should use time_scale *) window#show () + external sys_time_task : unit -> unit = "sim_sys_time_task" external periodic_task : unit -> unit = "sim_periodic_task" external nav_task : unit -> unit = "sim_nav_task" external monitor_task : unit -> unit = "sim_monitor_task" @@ -195,6 +197,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct Stdlib.timer ~scale:time_scale periodic_period periodic_task; Stdlib.timer ~scale:time_scale nav_period nav_task; Stdlib.timer ~scale:time_scale monitor_period monitor_task; + Stdlib.timer ~scale:time_scale sys_time_period sys_time_task; (* Forward or broacast messages according to "link" mode *) Hashtbl.iter From 6250317f11ce73b9a0be224d5b973e752a4e336f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 19 Nov 2014 20:22:36 +0100 Subject: [PATCH 46/86] [rotorcraft] datalink: check ac_id of RC_4CH message should fix at least some of the problems described in #965 --- sw/airborne/firmwares/rotorcraft/datalink.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 0bc0f349a4e..b2ec1c871ce 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -128,15 +128,16 @@ void dl_parse_msg(void) { DL_RC_3CH_pitch(dl_buffer)); break; case DL_RC_4CH : + if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { #ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_4ch_datalink( - DL_RC_4CH_mode(dl_buffer), - DL_RC_4CH_throttle(dl_buffer), - DL_RC_4CH_roll(dl_buffer), - DL_RC_4CH_pitch(dl_buffer), - DL_RC_4CH_yaw(dl_buffer)); + parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer), + DL_RC_4CH_throttle(dl_buffer), + DL_RC_4CH_roll(dl_buffer), + DL_RC_4CH_pitch(dl_buffer), + DL_RC_4CH_yaw(dl_buffer)); + } break; #endif // RADIO_CONTROL_TYPE_DATALINK #if defined GPS_DATALINK From f3845efcd93e4984238e6ddbd5b07d14c6fe81ee Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 20 Nov 2014 14:12:01 +0100 Subject: [PATCH 47/86] [modules] config_mkk_v2 settings to module --- conf/airframes/CDW/conf.xml | 6 ++++-- conf/modules/configure_actuators_mkk_v2.xml | 16 ++++++++++++++++ conf/settings/modules/config_mkk_v2.xml | 19 ------------------- 3 files changed, 20 insertions(+), 21 deletions(-) delete mode 100644 conf/settings/modules/config_mkk_v2.xml diff --git a/conf/airframes/CDW/conf.xml b/conf/airframes/CDW/conf.xml index c8ba362e02e..8dabbae903c 100644 --- a/conf/airframes/CDW/conf.xml +++ b/conf/airframes/CDW/conf.xml @@ -6,7 +6,8 @@ radio="radios/R6107SP_7ch.xml" telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic.xml" - settings=" settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/modules/config_asctec_v2.xml settings/modules/imu_quality_assessment.xml" + settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/modules/config_asctec_v2.xml" + settings_modules="modules/imu_quality_assessment.xml modules/gps_ubx_ucenter.xml" gui_color="white" /> diff --git a/conf/modules/configure_actuators_mkk_v2.xml b/conf/modules/configure_actuators_mkk_v2.xml index ccfa27a035c..b162c9c743a 100644 --- a/conf/modules/configure_actuators_mkk_v2.xml +++ b/conf/modules/configure_actuators_mkk_v2.xml @@ -4,6 +4,22 @@ Configure Mikrokopter MKK v2.0 BLDC motor controllers (requires subsystem actuators_mkk_v2) + + + + + + + + + + + + + + + +
diff --git a/conf/settings/modules/config_mkk_v2.xml b/conf/settings/modules/config_mkk_v2.xml deleted file mode 100644 index c78425f2b1c..00000000000 --- a/conf/settings/modules/config_mkk_v2.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - From bf0a52af9422b8364364a457f3ac22a59a3a29e3 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 20 Nov 2014 14:13:36 +0100 Subject: [PATCH 48/86] [OBC2014] Irrevisible failsafe on RC loss made optional in OBC rules --- sw/airborne/firmwares/fixedwing/main_fbw.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 2f15979bbb7..701bda65aa6 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -314,8 +314,11 @@ void periodic_task_fbw( void ) { #if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP #warning WARNING DANGER: OUTBACK_CHALLENGE RULE RC_LOST_NO_AP defined. If you loose RC you will NOT go to automatically go to AUTO2 Anymore!! set_failsafe_mode(); +#if OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE +#warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored commands[COMMAND_FORCECRASH] = 9600; -#else +#endif + #else fbw_mode = FBW_MODE_AUTO; #endif } From 88c95ce63be5e166f92b3f5f5c8a2737998c724d Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 20 Nov 2014 14:24:57 +0100 Subject: [PATCH 49/86] [conf] fix/part1 --- conf/airframes/CDW/TwoSeas.xml | 1 + conf/airframes/CDW/conf_two_seas.xml | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/conf/airframes/CDW/TwoSeas.xml b/conf/airframes/CDW/TwoSeas.xml index 049bf8775d3..1fc126570ca 100644 --- a/conf/airframes/CDW/TwoSeas.xml +++ b/conf/airframes/CDW/TwoSeas.xml @@ -37,6 +37,7 @@ + diff --git a/conf/airframes/CDW/conf_two_seas.xml b/conf/airframes/CDW/conf_two_seas.xml index 99eac6a9a55..1298732f52f 100644 --- a/conf/airframes/CDW/conf_two_seas.xml +++ b/conf/airframes/CDW/conf_two_seas.xml @@ -6,7 +6,8 @@ radio="radios/T8FG_SBUS.xml" telemetry="telemetry/default_fixedwing.xml" flight_plan="flight_plans/versatile.xml" - settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" + settings="settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml" + settings_modules="modules/light.xml" gui_color="blue" /> From ed9e970a0ca6c9e2bfaefa477d931c5f0bdea49c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 20 Nov 2014 14:30:48 +0100 Subject: [PATCH 50/86] fix radio_control sbus_dual subsystem --- .../subsystems/shared/radio_control_sbus_dual.makefile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile b/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile index 2a38812ff54..65b4249634f 100644 --- a/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile @@ -14,7 +14,7 @@ ifneq ($(RADIO_CONTROL_LED),none) ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) endif -RC_CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus.h\" +RC_CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus_dual.h\" RC_FBW_CFLAGS += -DRADIO_CONTROL @@ -28,7 +28,6 @@ RC_FBW_CFLAGS += -DUSE_$(SBUS1_PORT_UPPER) -D$(SBUS1_PORT_UPPER)_BAUD=B100000 RC_FBW_CFLAGS += -DUSE_$(SBUS2_PORT_UPPER) -D$(SBUS2_PORT_UPPER)_BAUD=B100000 RC_FBW_CFLAGS += -DSBUS1_UART_DEV=$(SBUS1_PORT_LOWER) RC_FBW_CFLAGS += -DSBUS2_UART_DEV=$(SBUS2_PORT_LOWER) -RC_FBW_CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus_dual.h\" RC_FBW_CFLAGS += -DRADIO_CONTROL_TYPE_SBUS RC_SRCS += $(SRC_SUBSYSTEMS)/radio_control.c RC_SRCS += $(SRC_SUBSYSTEMS)/radio_control/sbus_dual.c From 60c9bd16c406a3d979dcab7c0993705ef2bf2808 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 20 Nov 2014 19:07:01 +0100 Subject: [PATCH 51/86] [imu] fix default/weak implementation of imu_periodic should fix #972 --- sw/airborne/subsystems/imu.c | 4 ++++ sw/airborne/subsystems/imu.h | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index b65a0a37cc7..6ef8c19d3d6 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -226,6 +226,10 @@ void imu_SetBodyToImuCurrent(float set) { // weak functions, used if not explicitly provided by implementation +void WEAK imu_scale_gyro(void) +{ +} + void WEAK imu_scale_gyro(struct Imu* _imu) { RATES_COPY(_imu->gyro_prev, _imu->gyro); diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index 38023a711af..8ccd518d941 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -32,9 +32,10 @@ #include "math/pprz_orientation_conversion.h" #include "generated/airframe.h" -/* must be defined by underlying hardware */ +/** must be defined by underlying hardware */ extern void imu_impl_init(void); -extern void WEAK imu_periodic(void); //Optional +/** optional. If not provided by implementation, empty function is used */ +extern void imu_periodic(void); /** abstract IMU interface providing fixed point interface */ struct Imu { From c61507317f1f9bb1fb82f34da2c3987d2c4b8d45 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 20 Nov 2014 21:32:31 +0100 Subject: [PATCH 52/86] [airframe] different radio_control in simulation --- conf/airframes/CDW/TwoSeas.xml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/conf/airframes/CDW/TwoSeas.xml b/conf/airframes/CDW/TwoSeas.xml index 1fc126570ca..71bcd57613c 100644 --- a/conf/airframes/CDW/TwoSeas.xml +++ b/conf/airframes/CDW/TwoSeas.xml @@ -23,6 +23,10 @@ + + + + From 8fd490f1e1e6d7bae9980dfabb2fec051fc00ea6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 20 Nov 2014 21:40:34 +0100 Subject: [PATCH 53/86] [imu] fix unbelievably stupid copy paste error --- sw/airborne/subsystems/imu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 6ef8c19d3d6..c7dcb09f721 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -226,7 +226,7 @@ void imu_SetBodyToImuCurrent(float set) { // weak functions, used if not explicitly provided by implementation -void WEAK imu_scale_gyro(void) +void WEAK imu_periodic(void) { } From e24a21ce1d78f4d9ba596aeb4c1f35909f9f9527 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 20 Nov 2014 22:03:01 +0100 Subject: [PATCH 54/86] [conf] fix OCaml sim freqs for some airframes --- conf/airframes/ENAC/fixed-wing/apogee.xml | 13 ++++++------- conf/airframes/examples/microjet_twog_aspirin.xml | 10 +++++----- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/apogee.xml b/conf/airframes/ENAC/fixed-wing/apogee.xml index a8687c32b08..be0f44ee21c 100644 --- a/conf/airframes/ENAC/fixed-wing/apogee.xml +++ b/conf/airframes/ENAC/fixed-wing/apogee.xml @@ -19,20 +19,19 @@ - - - - - - - + + + + + + diff --git a/conf/airframes/examples/microjet_twog_aspirin.xml b/conf/airframes/examples/microjet_twog_aspirin.xml index d89003ecab9..02394e18458 100644 --- a/conf/airframes/examples/microjet_twog_aspirin.xml +++ b/conf/airframes/examples/microjet_twog_aspirin.xml @@ -10,7 +10,11 @@ - + + + + + @@ -20,10 +24,6 @@ - - - - From 1080da88d2fe001a9ba67b65c999018de0e5b3fa Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sun, 16 Nov 2014 20:35:20 +0100 Subject: [PATCH 55/86] [gps] Add Furuno NMEA based GPS --- .../subsystems/fixedwing/gps_furuno.makefile | 32 ++ .../subsystems/rotorcraft/gps_furuno.makefile | 24 + sw/airborne/subsystems/gps/gps_furuno.c | 109 +++++ sw/airborne/subsystems/gps/gps_nmea.c | 419 +++++++----------- sw/airborne/subsystems/gps/gps_nmea.h | 32 +- 5 files changed, 353 insertions(+), 263 deletions(-) create mode 100644 conf/firmwares/subsystems/fixedwing/gps_furuno.makefile create mode 100644 conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile create mode 100644 sw/airborne/subsystems/gps/gps_furuno.c diff --git a/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile new file mode 100644 index 00000000000..15577f5d712 --- /dev/null +++ b/conf/firmwares/subsystems/fixedwing/gps_furuno.makefile @@ -0,0 +1,32 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Furuno NMEA GPS unit + +GPS_LED ?= none + +ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DUSE_$(GPS_PORT) +ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DNMEA_PARSE_PROP + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +jsbsim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +jsbsim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c + +nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG +nps.srcs += $(SRC_SUBSYSTEMS)/gps.c +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c diff --git a/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile new file mode 100644 index 00000000000..7d12d6adb23 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile @@ -0,0 +1,24 @@ +# Hey Emacs, this is a -*- makefile -*- + +# Furuno NMEA GPS unit + + +ap.CFLAGS += -DUSE_GPS +ap.CFLAGS += -DGPS_LINK=$(GPS_PORT) +ap.CFLAGS += -DUSE_$(GPS_PORT) +ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) +ap.CFLAGS += -DNMEA_PARSE_PROP + +ifneq ($(GPS_LED),none) + ap.CFLAGS += -DGPS_LED=$(GPS_LED) +endif + +ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\" +ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c + +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c + +nps.CFLAGS += -DUSE_GPS +nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" +nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c + diff --git a/sw/airborne/subsystems/gps/gps_furuno.c b/sw/airborne/subsystems/gps/gps_furuno.c new file mode 100644 index 00000000000..befb57da650 --- /dev/null +++ b/sw/airborne/subsystems/gps/gps_furuno.c @@ -0,0 +1,109 @@ +/* + * + * Copyright (C) 2014 Freek van Tienen + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** + * @file subsystems/gps/gps_furuno.c + * GPS furuno based NMEA parser + */ + +#include "gps_nmea.h" +#include "subsystems/gps.h" +#include +#include + +#define GPS_FURUNO_SETTINGS_NB 10 +static const char *gps_furuno_settings[GPS_FURUNO_SETTINGS_NB] = { + "PERDAPI,FIXPERSEC,5", // Receive position every 5 Hz + "PERDAPI,CROUT,ALLOFF", // Disable all propriarty output + "PERDCFG,NMEAOUT,GGA,1", // Enable GGA every fix + "PERDCFG,NMEAOUT,RMC,1", // Enable RMC every fix + "PERDCFG,NMEAOUT,GSA,1", // Enable GSA every fix + "PERDCFG,NMEAOUT,GNS,0", // Disable GSA + "PERDCFG,NMEAOUT,ZDA,0", // Disable ZDA + "PERDCFG,NMEAOUT,GSV,0", // Disable GSV + "PERDCFG,NMEAOUT,GST,0", // Disable ZDA + "PERDAPI,CROUT,V" // Enable raw velocity +}; + +static void nmea_parse_perdcrv(void); + +void nmea_parse_prop_init(void) +{ + static uint8_t i = 0; + uint8_t j, len, crc; + char buf[128]; + + // Return when doen + if (i == GPS_FURUNO_SETTINGS_NB) { + return; + } + + for (; i < GPS_FURUNO_SETTINGS_NB; i++) { + len = strlen(gps_furuno_settings[i]); + crc = nmea_calc_crc(gps_furuno_settings[i], len); + sprintf(buf, "$%s*%02X\r\n", gps_furuno_settings[i], crc); + + // Check if there is enough space to send the config msg + if (GpsLink(CheckFreeSpace(len + 6))) { + for (j = 0; j < len + 6; j++) { + GpsLink(Transmit(buf[j])); + } + } else { + break; + } + } +} + +void nmea_parse_prop_msg(void) +{ + if (gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "PERDCRV", 7)) { + nmea_parse_perdcrv(); + } +} + +void nmea_parse_perdcrv(void) +{ + int i = 8; + + // Ignore reserved + nmea_read_until(&i); + + // Ignore reserved + nmea_read_until(&i); + + //EAST VEL + double east_vel = strtod(&gps_nmea.msg_buf[i], NULL); + gps.ned_vel.y = east_vel * 100; // in cm/s + + // Ignore reserved + nmea_read_until(&i); + + // NORTH VEL + double north_vel = strtod(&gps_nmea.msg_buf[i], NULL); + gps.ned_vel.x = north_vel * 100; // in cm/s + + //Convert velocity to ecef + struct LtpDef_i ltp; + ltp_def_from_ecef_i(<p, &gps.ecef_pos); + ecef_of_ned_vect_i(&gps.ecef_vel, <p, &gps.ned_vel); +} diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 1190294c515..987ebe2a82a 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -1,6 +1,7 @@ /* * * Copyright (C) 2008-2011 The Paparazzi Team + * 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -45,62 +46,139 @@ #include #include #include -#ifdef DEBUG_NMEA -// do debug-output if run on the DEBUG_NMEA-target - -#endif - struct GpsNmea gps_nmea; -void parse_nmea_GPGSA(void); -void parse_nmea_GPRMC(void); -void parse_nmea_GPGGA(void); +static void nmea_parse_GSA(void); +static void nmea_parse_RMC(void); +static void nmea_parse_GGA(void); -void gps_impl_init( void ) { +void gps_impl_init(void) +{ gps_nmea.msg_available = FALSE; gps_nmea.pos_available = FALSE; gps_nmea.gps_nb_ovrn = 0; gps_nmea.msg_len = 0; + nmea_parse_prop_init(); +} + +void WEAK nmea_parse_prop_init(void) +{ +} + +void WEAK nmea_parse_prop_msg(void) +{ +} + +/** + * parse_nmea_char() has a complete line. + * Find out what type of message it is and + * hand it to the parser for that type. + */ +void nmea_parse_msg(void) +{ + + if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "RMC", 3)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("parsing RMC: \"%s\" \n\r", gps_nmea.msg_buf); + NMEA_PRINT("RMC"); + nmea_parse_RMC(); + } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GGA", 3)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r", gps_nmea.msg_buf); + NMEA_PRINT("GGA"); + nmea_parse_GGA(); + } else if (gps_nmea.msg_len > 5 && !strncmp(&gps_nmea.msg_buf[2] , "GSA", 3)) { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("GSA: \"%s\" \n\r", gps_nmea.msg_buf); + NMEA_PRINT("GSA"); + nmea_parse_GSA(); + } else { + gps_nmea.msg_buf[gps_nmea.msg_len] = 0; + NMEA_PRINT("Propriarty message: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf); + nmea_parse_prop_msg(); + } + + // reset message-buffer + gps_nmea.msg_len = 0; +} + + +/** + * This is the actual parser. + * It reads one character at a time + * setting gps_nmea.msg_available to TRUE + * after a full line. + */ +void nmea_parse_char(uint8_t c) +{ + //reject empty lines + if (gps_nmea.msg_len == 0) { + if (c == '\r' || c == '\n' || c == '$') { + return; + } + } + + // fill the buffer, unless it's full + if (gps_nmea.msg_len < NMEA_MAXLEN - 1) { + + // messages end with a linefeed + //AD: TRUNK: if (c == '\r' || c == '\n') + if (c == '\r' || c == '\n') { + gps_nmea.msg_available = TRUE; + } else { + gps_nmea.msg_buf[gps_nmea.msg_len] = c; + gps_nmea.msg_len ++; + } + } + + if (gps_nmea.msg_len >= NMEA_MAXLEN - 1) { + gps_nmea.msg_available = TRUE; + } } +/** + * Calculate control sum of binary buffer + */ +uint8_t nmea_calc_crc(const char *buff, int buff_sz) +{ + uint8_t chsum = 0, + it; + + for (it = 0; it < buff_sz; ++it) { + chsum ^= buff[it]; + } + + return chsum; +} /** * parse GPGSA-nmea-messages stored in * nmea_msg_buf . */ -void parse_nmea_GPGSA(void) { +static void nmea_parse_GSA(void) +{ int i = 6; // current position in the message, start after: GPGSA, - // char* endptr; // end of parsed substrings // attempt to reject empty packets right away - if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { NMEA_PRINT("p_GPGSA() - skipping empty message\n\r"); return; } // get auto2D/3D // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: fix - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); // get 2D/3D-fix // set gps_mode=3=3d, 2=2d, 1=no fix or 0 gps.fix = atoi(&gps_nmea.msg_buf[i]); - if (gps.fix == 1) + if (gps.fix == 1) { gps.fix = 0; - NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix); - while(gps_nmea.msg_buf[i++] != ',') { // next field:satellite-number-0 - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGSA() - skipping incomplete message\n\r"); - return; - } } + NMEA_PRINT("p_GPGSA() - gps.fix=%i (3=3D)\n\r", gps.fix); + nmea_read_until(&i); //int satcount = 0; @@ -111,78 +189,43 @@ void parse_nmea_GPGSA(void) { * parse GPRMC-nmea-messages stored in * gps_nmea.msg_buf . */ -void parse_nmea_GPRMC(void) { +static void nmea_parse_RMC(void) +{ int i = 6; // current position in the message, start after: GPRMC, - char* endptr; // end of parsed substrings // attempt to reject empty packets right away - if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { NMEA_PRINT("p_GPRMC() - skipping empty message\n\r"); return; } - - // get time - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: warning - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + // First read time (ignored) // get warning - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: lat - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); + // get lat - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); + // get North/South - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: lon - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); + // get lon - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); + // get eath/west - // ignored - while(gps_nmea.msg_buf[i++] != ',') { // next field: speed - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } + nmea_read_until(&i); + // get speed - double speed = strtod(&gps_nmea.msg_buf[i], &endptr); - gps.gspeed = speed * 1.852 * 100 / (60*60); - NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed*1000), (gps.gspeed*1000)); - while(gps_nmea.msg_buf[i++] != ',') { // next field: course - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPRMC() - skipping incomplete message\n\r"); - return; - } - } - double course = strtod(&gps_nmea.msg_buf[i], &endptr); + nmea_read_until(&i); + double speed = strtod(&gps_nmea.msg_buf[i], NULL); + gps.gspeed = speed * 1.852 * 100 / (60 * 60); + NMEA_PRINT("p_GPRMC() - ground-speed=%d knot = %d cm/s\n\r", (speed * 1000), (gps.gspeed * 1000)); + + // get course + nmea_read_until(&i); + double course = strtod(&gps_nmea.msg_buf[i], NULL); gps.course = RadOfDeg(course) * 1e7; - NMEA_PRINT("COURSE: %d \n\r",gps_course); + NMEA_PRINT("COURSE: %d \n\r", gps_course); } @@ -190,95 +233,66 @@ void parse_nmea_GPRMC(void) { * parse GPGGA-nmea-messages stored in * gps_nmea.msg_buf . */ -void parse_nmea_GPGGA(void) { +static void nmea_parse_GGA(void) +{ int i = 6; // current position in the message, start after: GPGGA, - char* endptr; // end of parsed substrings double degrees, minutesfrac; struct LlaCoor_f lla_f; // attempt to reject empty packets right away - if(gps_nmea.msg_buf[i]==',' && gps_nmea.msg_buf[i+1]==',') { + if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { NMEA_PRINT("p_GPGGA() - skipping empty message\n\r"); return; } // get UTC time [hhmmss.sss] - // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], &endptr); + // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL); // FIXME: parse UTC time correctly - double time = strtod(&gps_nmea.msg_buf[i],&endptr); - gps.tow = (uint32_t)((time+1)*1000); - - //AD TODO: strtod itow - while(gps_nmea.msg_buf[i++] != ',') { // next field: latitude - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); - return; - } - } + double time = strtod(&gps_nmea.msg_buf[i], NULL); + gps.tow = (uint32_t)((time + 1) * 1000); // get latitude [ddmm.mmmmm] - double lat = strtod(&gps_nmea.msg_buf[i], &endptr); + nmea_read_until(&i); + double lat = strtod(&gps_nmea.msg_buf[i], NULL); // convert to pure degrees [dd.dddd] format - minutesfrac = modf(lat/100, °rees); - lat = degrees + (minutesfrac*100)/60; - // convert to radians - //GpsInfo.PosLLA.lat.f *= (M_PI/180); + minutesfrac = modf(lat / 100, °rees); + lat = degrees + (minutesfrac * 100) / 60; - while(gps_nmea.msg_buf[i++] != ',') { // next field: N/S indicator - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); - return; - } - } - - // correct latitute for N/S - if(gps_nmea.msg_buf[i] == 'S') + // get latitute N/S + nmea_read_until(&i); + if (gps_nmea.msg_buf[i] == 'S') { lat = -lat; + } // convert to radians lla_f.lat = RadOfDeg(lat); - gps.lla_pos.lat = lat * 1e7; // convert to fixed-point - NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), lla_f.lat); + NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat * 1000), lla_f.lat); - while(gps_nmea.msg_buf[i++] != ',') { // next field: longitude - if (i >= gps_nmea.msg_len) - return; - } - // get longitude [ddmm.mmmmm] - double lon = strtod(&gps_nmea.msg_buf[i], &endptr); + nmea_read_until(&i); + double lon = strtod(&gps_nmea.msg_buf[i], NULL); // convert to pure degrees [dd.dddd] format - minutesfrac = modf(lon/100, °rees); - lon = degrees + (minutesfrac*100)/60; - // convert to radians - //GpsInfo.PosLLA.lon.f *= (M_PI/180); - while(gps_nmea.msg_buf[i++] != ',') { // next field: E/W indicator - if (i >= gps_nmea.msg_len) - return; - } + minutesfrac = modf(lon / 100, °rees); + lon = degrees + (minutesfrac * 100) / 60; - // correct latitute for E/W - if(gps_nmea.msg_buf[i] == 'W') + // get longitude E/W + nmea_read_until(&i); + if (gps_nmea.msg_buf[i] == 'W') { lon = -lon; + } // convert to radians lla_f.lon = RadOfDeg(lon); - gps.lla_pos.lon = lon * 1e7; // convert to fixed-point - NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), lla_f.lon, gps.tow); - - - while(gps_nmea.msg_buf[i++] != ',') { // next field: position fix status - if (i >= gps_nmea.msg_len) - return; - } + NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow); - // position fix status + // get position fix status + nmea_read_until(&i); // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS // check for good position fix - if( (gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',') ) { + if ((gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',')) { gps_nmea.pos_available = TRUE; NMEA_PRINT("p_GPGGA() - POS_AVAILABLE == TRUE\n\r"); } else { @@ -286,61 +300,34 @@ void parse_nmea_GPGGA(void) { NMEA_PRINT("p_GPGGA() - gps_pos_available == false\n\r"); } - while(gps_nmea.msg_buf[i++] != ',') { // next field: satellites used - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r\r"); - return; - } - } // get number of satellites used in GPS solution + nmea_read_until(&i); gps.num_sv = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GPGGA() - gps_numSatlitesUsed=%i\n\r", gps.num_sv); - while(gps_nmea.msg_buf[i++] != ',') { // next field: HDOP (horizontal dilution of precision) - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); - return; - } - } // we use HDOP here, as the PDOP is not in the message - float hdop = strtof(&gps_nmea.msg_buf[i], &endptr); + nmea_read_until(&i); + float hdop = strtof(&gps_nmea.msg_buf[i], NULL); gps.pdop = hdop * 100; - while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude - if (i >= gps_nmea.msg_len) { - NMEA_PRINT("p_GPGGA() - skipping incomplete message\n\r"); - return; - } - } // get altitude (in meters) above geoid (MSL) + nmea_read_until(&i); // lla_f.alt should actuall be height above ellipsoid, // but since we don't get that, use hmsl instead - lla_f.alt = strtof(&gps_nmea.msg_buf[i], &endptr); + lla_f.alt = strtof(&gps_nmea.msg_buf[i], NULL); gps.hmsl = lla_f.alt * 1000; gps.lla_pos.alt = gps.hmsl; NMEA_PRINT("p_GPGGA() - gps_alt=%i\n\r", gps.hmsl); - while(gps_nmea.msg_buf[i++] != ',') { // next field: altitude units, always 'M' - if (i >= gps_nmea.msg_len) - return; - } - while(gps_nmea.msg_buf[i++] != ',') { // next field: geoid seperation - if (i >= gps_nmea.msg_len) - return; - } - while(gps_nmea.msg_buf[i++] != ',') { // next field: seperation units - if (i >= gps_nmea.msg_len) - return; - } - while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS age - if (i >= gps_nmea.msg_len) - return; - } - while(gps_nmea.msg_buf[i++] != ',') { // next field: DGPS station ID - if (i >= gps_nmea.msg_len) - return; - } - //while(gps_nmea.msg_buf[i++] != '*'); // next field: checksum + // get altitude units (allways M) + nmea_read_until(&i); + // get geoid seperation + nmea_read_until(&i); + // get seperations units + nmea_read_until(&i); + // get DGPS age + nmea_read_until(&i); + // get DGPS station ID #if GPS_USE_LATLONG /* convert to utm */ @@ -349,8 +336,8 @@ void parse_nmea_GPGGA(void) { utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif @@ -362,71 +349,3 @@ void parse_nmea_GPGGA(void) { gps.ecef_pos.y = ecef_f.y * 100; gps.ecef_pos.z = ecef_f.z * 100; } - -/** - * parse_nmea_char() has a complete line. - * Find out what type of message it is and - * hand it to the parser for that type. - */ -void nmea_parse_msg( void ) { - - if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPRMC", 5)) { - gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("parsing RMC: \"%s\" \n\r",gps_nmea.msg_buf); - NMEA_PRINT("RMC"); - parse_nmea_GPRMC(); - } - else { - if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGGA", 5)) { - gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("parse_gps_msg() - parsing GGA gps-message \"%s\" \n\r",gps_nmea.msg_buf); - NMEA_PRINT("GGA"); - parse_nmea_GPGGA(); - } - else { - if(gps_nmea.msg_len > 5 && !strncmp(gps_nmea.msg_buf , "GPGSA", 5)) { - gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("GSA: \"%s\" \n\r",gps_nmea.msg_buf); - NMEA_PRINT("GSA"); - parse_nmea_GPGSA(); - } else { - gps_nmea.msg_buf[gps_nmea.msg_len] = 0; - NMEA_PRINT("ignoring: len=%i \n\r \"%s\" \n\r", gps_nmea.msg_len, gps_nmea.msg_buf); - } - } - } - - // reset message-buffer - gps_nmea.msg_len = 0; -} - - -/** - * This is the actual parser. - * It reads one character at a time - * setting gps_nmea.msg_available to TRUE - * after a full line. - */ -void nmea_parse_char( uint8_t c ) { - //reject empty lines - if (gps_nmea.msg_len == 0) { - if (c == '\r' || c == '\n' || c == '$') - return; - } - - // fill the buffer, unless it's full - if (gps_nmea.msg_len < NMEA_MAXLEN - 1) { - - // messages end with a linefeed - //AD: TRUNK: if (c == '\r' || c == '\n') - if (c == '\r' || c == '\n') { - gps_nmea.msg_available = TRUE; - } else { - gps_nmea.msg_buf[gps_nmea.msg_len] = c; - gps_nmea.msg_len ++; - } - } - - if (gps_nmea.msg_len >= NMEA_MAXLEN - 1) - gps_nmea.msg_available = TRUE; -} diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index 0772e68b90e..cadece9d0b5 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2004-2011 The Paparazzi Team + * 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -31,11 +32,7 @@ #include "mcu_periph/uart.h" -#define GPS_NB_CHANNELS 16 - -#ifdef DEBUG_NMEA -#define NMEA_PRINT(...) { UsbSPrintString( __VA_ARGS__);}; -#else +#ifndef DEBUG_NMEA #define NMEA_PRINT(...) {}; #endif @@ -62,6 +59,7 @@ extern struct GpsNmea gps_nmea; #define GpsBuffer() GpsLink(ChAvailable()) #define GpsEvent(_sol_available_callback) { \ + nmea_parse_prop_init(); \ if (GpsBuffer()) { \ ReadGpsBuffer(); \ } \ @@ -80,19 +78,27 @@ extern struct GpsNmea gps_nmea; } \ } -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_nmea.msg_available) \ - nmea_parse_char(GpsLink(Getch())); \ +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_nmea.msg_available) \ + nmea_parse_char(GpsLink(Getch())); \ } - /** The function to be called when a characted friom the device is available */ extern void nmea_parse_char(uint8_t c); - extern void nmea_parse_msg(void); - - -#define gps_nmea_Reset(_val) { } +extern uint8_t nmea_calc_crc(const char *buff, int buff_sz); +void nmea_parse_prop_init(void); +void nmea_parse_prop_msg(void); + +/** Read until a certain character, placed here for proprietary includes */ +static inline void nmea_read_until(int *i) +{ + while (gps_nmea.msg_buf[*i++] != ',') { + if (*i >= gps_nmea.msg_len) { + return; + } + } +} #endif /* GPS_NMEA_H */ From b474c5e126edba8bc29df9015a56099719678590 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 19 Nov 2014 14:17:07 +0100 Subject: [PATCH 56/86] [omap] Added linux i2c driver closes #961 --- sw/airborne/arch/omap/mcu_periph/i2c_arch.c | 100 ++++++++--- sw/airborne/arch/omap/mcu_periph/i2c_arch.h | 13 +- .../omap/mcu_periph/i2c_smbus.h} | 157 +++++++++--------- sw/airborne/boards/ardrone/electrical_raw.c | 41 ++--- 4 files changed, 176 insertions(+), 135 deletions(-) rename sw/airborne/{boards/ardrone/i2c-dev.h => arch/omap/mcu_periph/i2c_smbus.h} (71%) diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c index d0de65700e0..3ae02d10120 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c @@ -1,6 +1,6 @@ /* * - * Copyright (C) 2009-2013 The Paparazzi Team + * Copyright (C) 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -22,58 +22,104 @@ */ /** @file arch/omap/mcu_periph/i2c_arch.h - * I2C functionality (unimplemented) + * I2C functionality */ #include "mcu_periph/i2c.h" +#include +#include +#include +#include +#include +#include + +void i2c_event(void) +{ +} +void i2c_setbitrate(struct i2c_periph *p __attribute__((unused)), int bitrate __attribute__((unused))) +{ +} -bool_t i2c_idle(struct i2c_periph *p __attribute__ ((unused))) { return TRUE; } -bool_t i2c_submit(struct i2c_periph* p __attribute__ ((unused)), struct i2c_transaction* t __attribute__ ((unused))) { return TRUE;} -void i2c_event(void) { } -void i2c_setbitrate(struct i2c_periph* p, int bitrate __attribute__ ((unused))) { } +bool_t i2c_idle(struct i2c_periph *p __attribute__((unused))) +{ + return TRUE; +} + +bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) +{ + int file = (int)p->reg_addr; + + // Set the slave address + ioctl(file, I2C_SLAVE, t->slave_addr); + + // Switch the different transaction types + switch (t->type) { + // Just transmitting + case I2CTransTx: + if (write(file, (uint8_t *)t->buf, t->len_w) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + // Just reading + case I2CTransRx: + if (read(file, (uint8_t *)t->buf, t->len_r) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + // First Transmit and then read + case I2CTransTxRx: + if (write(file, (uint8_t *)t->buf, t->len_w) < 0 || read(file, (uint8_t *)t->buf, t->len_r) < 0) { + t->status = I2CTransFailed; + return TRUE; + } + break; + default: + break; + } + + // Successfull transfer + t->status = I2CTransSuccess; + return TRUE; +} #if USE_I2C0 struct i2c_errors i2c0_errors; -void i2c0_hw_init(void) { +void i2c0_hw_init(void) +{ + i2c1.reg_addr = (void *)open("/dev/i2c-0", O_RDWR); i2c0.errors = &i2c0_errors; - ZEROS_ERR_COUNTER(i2c0_errors); -} -void i2c0_ev_isr(void) { -} - -void i2c0_er_isr(void) { + /* zeros error counter */ + ZEROS_ERR_COUNTER(i2c0_errors); } #endif #if USE_I2C1 struct i2c_errors i2c1_errors; -void i2c1_hw_init(void) { +void i2c1_hw_init(void) +{ + i2c1.reg_addr = (void *)open("/dev/i2c-1", O_RDWR); i2c1.errors = &i2c1_errors; - ZEROS_ERR_COUNTER(i2c1_errors); -} -void i2c1_ev_isr(void) { -} - -void i2c1_er_isr(void) { + /* zeros error counter */ + ZEROS_ERR_COUNTER(i2c1_errors); } #endif #if USE_I2C2 struct i2c_errors i2c2_errors; -void i2c2_hw_init(void) { +void i2c2_hw_init(void) +{ + i2c2.reg_addr = (void *)open("/dev/i2c-2", O_RDWR); i2c2.errors = &i2c2_errors; - ZEROS_ERR_COUNTER(i2c2_errors); -} -void i2c2_ev_isr(void) { -} - -void i2c2_er_isr(void) { + /* zeros error counter */ + ZEROS_ERR_COUNTER(i2c2_errors); } #endif diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_arch.h b/sw/airborne/arch/omap/mcu_periph/i2c_arch.h index 2d1a835e79f..0be85364cdb 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_arch.h +++ b/sw/airborne/arch/omap/mcu_periph/i2c_arch.h @@ -1,6 +1,6 @@ /* * - * Copyright (C) 2009-2013 The Paparazzi Team + * Copyright (C) 2014 Freek van Tienen * * This file is part of paparazzi. * @@ -22,7 +22,7 @@ */ /** @file arch/omap/mcu_periph/i2c_arch.h - * I2C functionality (unimplemented) + * I2C functionality */ #ifndef OMAP_MCU_PERIPH_I2C_ARCH_H @@ -30,27 +30,18 @@ #include "mcu_periph/i2c.h" -#define I2cSendStart() {} - - #if USE_I2C0 - extern void i2c0_hw_init(void); - #endif /* USE_I2C0 */ #if USE_I2C1 - extern void i2c1_hw_init(void); - #endif /* USE_I2C1 */ #if USE_I2C2 - extern void i2c2_hw_init(void); - #endif /* USE_I2C2 */ diff --git a/sw/airborne/boards/ardrone/i2c-dev.h b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h similarity index 71% rename from sw/airborne/boards/ardrone/i2c-dev.h rename to sw/airborne/arch/omap/mcu_periph/i2c_smbus.h index 79985d2a6b6..77cd9575ea5 100644 --- a/sw/airborne/boards/ardrone/i2c-dev.h +++ b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h @@ -31,6 +31,7 @@ #define LIB_I2CDEV_H #include +#include #include /* @@ -39,12 +40,12 @@ struct i2c_msg { __u16 addr; /* slave address */ unsigned short flags; -#define I2C_M_TEN 0x10 /* we have a ten bit chip address */ -#define I2C_M_RD 0x01 -#define I2C_M_NOSTART 0x4000 +#define I2C_M_TEN 0x10 /* we have a ten bit chip address */ +#define I2C_M_RD 0x01 +#define I2C_M_NOSTART 0x4000 #define I2C_M_REV_DIR_ADDR 0x2000 -#define I2C_M_IGNORE_NAK 0x1000 -#define I2C_M_NO_RD_ACK 0x0800 +#define I2C_M_IGNORE_NAK 0x1000 +#define I2C_M_NO_RD_ACK 0x0800 short len; /* msg length */ char *buf; /* pointer to msg data */ }; @@ -92,7 +93,7 @@ union i2c_smbus_data { __u8 byte; __u16 word; __u8 block[I2C_SMBUS_BLOCK_MAX + 2]; /* block[0] is used for length */ - /* and one more for PEC */ + /* and one more for PEC */ }; /* smbus_access read or write markers */ @@ -117,20 +118,20 @@ union i2c_smbus_data { * dependent layers - these can be listed here, or see the * corresponding header files. */ - /* -> bit-adapter specific ioctls */ +/* -> bit-adapter specific ioctls */ #define I2C_RETRIES 0x0701 /* number of times a device address */ - /* should be polled when not */ - /* acknowledging */ +/* should be polled when not */ +/* acknowledging */ #define I2C_TIMEOUT 0x0702 /* set timeout - call with int */ /* this is for i2c-dev.c */ #define I2C_SLAVE 0x0703 /* Change slave address */ - /* Attn.: Slave address is 7 or 10 bits */ +/* Attn.: Slave address is 7 or 10 bits */ #define I2C_SLAVE_FORCE 0x0706 /* Change slave address */ - /* Attn.: Slave address is 7 or 10 bits */ - /* This changes the address, even if it */ - /* is already taken! */ +/* Attn.: Slave address is 7 or 10 bits */ +/* This changes the address, even if it */ +/* is already taken! */ #define I2C_TENBIT 0x0704 /* 0 for 7 bit addrs, != 0 for 10 bit */ #define I2C_FUNCS 0x0705 /* Get the adapter functionality */ @@ -144,20 +145,6 @@ union i2c_smbus_data { /* Note: 10-bit addresses are NOT supported! */ -/* This is the structure as used in the I2C_SMBUS ioctl call */ -struct i2c_smbus_ioctl_data { - char read_write; - __u8 command; - int size; - union i2c_smbus_data *data; -}; - -/* This is the structure as used in the I2C_RDWR ioctl call */ -struct i2c_rdwr_ioctl_data { - struct i2c_msg *msgs; /* pointers to i2c_msgs */ - int nmsgs; /* number of i2c_msgs */ -}; - static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, int size, union i2c_smbus_data *data) @@ -168,65 +155,68 @@ static inline __s32 i2c_smbus_access(int file, char read_write, __u8 command, args.command = command; args.size = size; args.data = data; - return ioctl(file,I2C_SMBUS,&args); + return ioctl(file, I2C_SMBUS, &args); } static inline __s32 i2c_smbus_write_quick(int file, __u8 value) { - return i2c_smbus_access(file,value,0,I2C_SMBUS_QUICK,NULL); + return i2c_smbus_access(file, value, 0, I2C_SMBUS_QUICK, NULL); } static inline __s32 i2c_smbus_read_byte(int file) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,0,I2C_SMBUS_BYTE,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, 0, I2C_SMBUS_BYTE, &data)) { return -1; - else + } else { return 0x0FF & data.byte; + } } static inline __s32 i2c_smbus_write_byte(int file, __u8 value) { - return i2c_smbus_access(file,I2C_SMBUS_WRITE,value, - I2C_SMBUS_BYTE,NULL); + return i2c_smbus_access(file, I2C_SMBUS_WRITE, value, + I2C_SMBUS_BYTE, NULL); } static inline __s32 i2c_smbus_read_byte_data(int file, __u8 command) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_BYTE_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_BYTE_DATA, &data)) { return -1; - else + } else { return 0x0FF & data.byte; + } } static inline __s32 i2c_smbus_write_byte_data(int file, __u8 command, - __u8 value) + __u8 value) { union i2c_smbus_data data; data.byte = value; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BYTE_DATA, &data); } static inline __s32 i2c_smbus_read_word_data(int file, __u8 command) { union i2c_smbus_data data; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_WORD_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_WORD_DATA, &data)) { return -1; - else + } else { return 0x0FFFF & data.word; + } } static inline __s32 i2c_smbus_write_word_data(int file, __u8 command, - __u16 value) + __u16 value) { union i2c_smbus_data data; data.word = value; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_WORD_DATA, &data); } @@ -234,41 +224,45 @@ static inline __s32 i2c_smbus_process_call(int file, __u8 command, __u16 value) { union i2c_smbus_data data; data.word = value; - if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, - I2C_SMBUS_PROC_CALL,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_WRITE, command, + I2C_SMBUS_PROC_CALL, &data)) { return -1; - else + } else { return 0x0FFFF & data.word; + } } /* Returns the number of read bytes */ static inline __s32 i2c_smbus_read_block_data(int file, __u8 command, - __u8 *values) + __u8 *values) { union i2c_smbus_data data; int i; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, - I2C_SMBUS_BLOCK_DATA,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, + I2C_SMBUS_BLOCK_DATA, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } static inline __s32 i2c_smbus_write_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_BLOCK_DATA, &data); } @@ -277,56 +271,63 @@ static inline __s32 i2c_smbus_write_block_data(int file, __u8 command, ask for less than 32 bytes, your code will only work with kernels 2.6.23 and later. */ static inline __s32 i2c_smbus_read_i2c_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; + } data.block[0] = length; - if (i2c_smbus_access(file,I2C_SMBUS_READ,command, + if (i2c_smbus_access(file, I2C_SMBUS_READ, command, length == 32 ? I2C_SMBUS_I2C_BLOCK_BROKEN : - I2C_SMBUS_I2C_BLOCK_DATA,&data)) + I2C_SMBUS_I2C_BLOCK_DATA, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } static inline __s32 i2c_smbus_write_i2c_block_data(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - return i2c_smbus_access(file,I2C_SMBUS_WRITE,command, + return i2c_smbus_access(file, I2C_SMBUS_WRITE, command, I2C_SMBUS_I2C_BLOCK_BROKEN, &data); } /* Returns the number of read bytes */ static inline __s32 i2c_smbus_block_process_call(int file, __u8 command, - __u8 length, __u8 *values) + __u8 length, __u8 *values) { union i2c_smbus_data data; int i; - if (length > 32) + if (length > 32) { length = 32; - for (i = 1; i <= length; i++) - data.block[i] = values[i-1]; + } + for (i = 1; i <= length; i++) { + data.block[i] = values[i - 1]; + } data.block[0] = length; - if (i2c_smbus_access(file,I2C_SMBUS_WRITE,command, - I2C_SMBUS_BLOCK_PROC_CALL,&data)) + if (i2c_smbus_access(file, I2C_SMBUS_WRITE, command, + I2C_SMBUS_BLOCK_PROC_CALL, &data)) { return -1; - else { - for (i = 1; i <= data.block[0]; i++) - values[i-1] = data.block[i]; + } else { + for (i = 1; i <= data.block[0]; i++) { + values[i - 1] = data.block[i]; + } return data.block[0]; } } diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c index 4f768b049c4..6a9d2488a30 100644 --- a/sw/airborne/boards/ardrone/electrical_raw.c +++ b/sw/airborne/boards/ardrone/electrical_raw.c @@ -35,7 +35,7 @@ #include #include #include -#include "i2c-dev.h" +#include "mcu_periph/i2c_smbus.h" #include "subsystems/commands.h" #include "generated/airframe.h" @@ -67,45 +67,48 @@ static struct { int fd; -void electrical_init(void) { +void electrical_init(void) +{ // First we try to kill the program.elf and its respawner if it is running (done here because initializes first) int ret = system("killall -9 program.elf.respawner.sh; killall -9 program.elf"); (void) ret; // Initialize 12c device for power - fd = open( "/dev/i2c-1", O_RDWR ); - if ( ioctl( fd, I2C_SLAVE_FORCE, 0x4a) < 0 ) { - fprintf( stderr, "Failed to set slave address: %m\n" ); + fd = open("/dev/i2c-1", O_RDWR); + if (ioctl(fd, I2C_SLAVE_FORCE, 0x4a) < 0) { + fprintf(stderr, "Failed to set slave address: %m\n"); } electrical_setup(); electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY; } -void electrical_setup(void) { +void electrical_setup(void) +{ // Turn on MADC in CTRL1 - if( i2c_smbus_write_byte_data( fd, 0x00, 0x01)) { - fprintf( stderr, "Failed to write to I2C device. 1\n" ); + if (i2c_smbus_write_byte_data(fd, 0x00, 0x01)) { + fprintf(stderr, "Failed to write to I2C device. 1\n"); } // Select ADCIN0 for conversion in SW1SELECT_LSB - if( i2c_smbus_write_byte_data( fd, 0x06, 0xff)){ - fprintf( stderr, "Failed to write to I2C device. 2\n" ); + if (i2c_smbus_write_byte_data(fd, 0x06, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 2\n"); } // Select ADCIN12 for conversion in SW1SELECT_MSB - if( i2c_smbus_write_byte_data( fd, 0x07, 0xff)) { - fprintf( stderr, "Failed to write to I2C device. 3\n" ); + if (i2c_smbus_write_byte_data(fd, 0x07, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 3\n"); } // Setup register for averaging - if( i2c_smbus_write_byte_data( fd, 0x08, 0xff)) { - fprintf( stderr, "Failed to write to I2C device. 4\n" ); + if (i2c_smbus_write_byte_data(fd, 0x08, 0xff)) { + fprintf(stderr, "Failed to write to I2C device. 4\n"); } // Start all channel conversion by setting bit 5 to one in CTRL_SW1 - if( i2c_smbus_write_byte_data( fd, 0x12, 0x20)) { - fprintf( stderr, "Failed to write to I2C device. 5\n" ); + if (i2c_smbus_write_byte_data(fd, 0x12, 0x20)) { + fprintf(stderr, "Failed to write to I2C device. 5\n"); } } -void electrical_periodic(void) { +void electrical_periodic(void) +{ electrical_setup(); @@ -122,7 +125,7 @@ void electrical_periodic(void) { //from raw measurement we got quite a lineair response //9.0V=662, 9.5V=698, 10.0V=737,10.5V=774, 11.0V=811, 11.5V=848, 12.0V=886, 12.5V=923 //leading to our 0.13595166 magic number for decivolts conversion - electrical.vsupply = raw_voltage*0.13595166; + electrical.vsupply = raw_voltage * 0.13595166; /* * Superellipse: abs(x/a)^n + abs(y/b)^n = 1 @@ -139,5 +142,5 @@ void electrical_periodic(void) { /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor */ - electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); + electrical.current = b - pow((pow(b, electrical_priv.nonlin_factor) - pow((b * x), electrical_priv.nonlin_factor)), (1. / electrical_priv.nonlin_factor)); } From eb38af898cce19be9cf2927bd90ff372dd64fa30 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Wed, 19 Nov 2014 13:31:33 +0100 Subject: [PATCH 57/86] [ms5611] Fix baro i2c transactions Since the simple i2c driver for linux does busy waiting on i2c_submit (and not queue a new transaction), setting the transaction status to done at the end (with a new transcaction in between) will set the status of the wrong transaction... Did not seem happen with the other i2c implementations (lpc21, stm32) so far, but since they add the transaction to the queue which is handled in the I2C ISR, it could also happen there (race condition). closes #960 --- sw/airborne/peripherals/ms5611_i2c.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c index 0127c25f1c1..5e22e5968c5 100644 --- a/sw/airborne/peripherals/ms5611_i2c.c +++ b/sw/airborne/peripherals/ms5611_i2c.c @@ -129,6 +129,7 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { ms->data.d1 = (ms->i2c_trans.buf[0] << 16) | (ms->i2c_trans.buf[1] << 8) | ms->i2c_trans.buf[2]; + ms->i2c_trans.status = I2CTransDone; if (ms->data.d1 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; @@ -146,6 +147,7 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { ms->data.d2 = (ms->i2c_trans.buf[0] << 16) | (ms->i2c_trans.buf[1] << 8) | ms->i2c_trans.buf[2]; + ms->i2c_trans.status = I2CTransDone; if (ms->data.d2 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; @@ -159,9 +161,9 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { break; default: + ms->i2c_trans.status = I2CTransDone; break; } - ms->i2c_trans.status = I2CTransDone; } } else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized @@ -178,6 +180,7 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { /* read prom data */ ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) | ms->i2c_trans.buf[1]; + ms->i2c_trans.status = I2CTransDone; if (ms->prom_cnt < PROM_NB) { /* get next prom data */ ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); @@ -195,10 +198,10 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { } } } - ms->i2c_trans.status = I2CTransDone; break; default: + ms->i2c_trans.status = I2CTransDone; break; } } From ffa4c8dc41bdc74cdf0a6813b61e74686fcd6a6f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 21 Nov 2014 14:54:36 +0100 Subject: [PATCH 58/86] [omap] i2c: disable some warnings and fix dox --- sw/airborne/arch/omap/mcu_periph/i2c_arch.c | 10 ++++++++-- sw/airborne/arch/omap/mcu_periph/i2c_smbus.h | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c index 3ae02d10120..f9f8b34d692 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/i2c_arch.c @@ -21,7 +21,7 @@ * */ -/** @file arch/omap/mcu_periph/i2c_arch.h +/** @file arch/omap/mcu_periph/i2c_arch.c * I2C functionality */ @@ -53,6 +53,9 @@ bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) // Set the slave address ioctl(file, I2C_SLAVE, t->slave_addr); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + // Switch the different transaction types switch (t->type) { // Just transmitting @@ -71,7 +74,8 @@ bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) break; // First Transmit and then read case I2CTransTxRx: - if (write(file, (uint8_t *)t->buf, t->len_w) < 0 || read(file, (uint8_t *)t->buf, t->len_r) < 0) { + if (write(file, (uint8_t *)t->buf, t->len_w) < 0 || + read(file, (uint8_t *)t->buf, t->len_r) < 0) { t->status = I2CTransFailed; return TRUE; } @@ -80,6 +84,8 @@ bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) break; } +#pragma GCC diagnostic pop + // Successfull transfer t->status = I2CTransSuccess; return TRUE; diff --git a/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h index 77cd9575ea5..92cc94e779f 100644 --- a/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h +++ b/sw/airborne/arch/omap/mcu_periph/i2c_smbus.h @@ -23,7 +23,7 @@ /* $Id: i2c-dev.h 5361 2008-10-19 09:47:02Z khali $ */ /** - * @file boards/ardrone/i2c-dev.h + * @file arch/omap/mcu_periph/i2c_smbus.h * I2C-bus driver */ From c046fa74291252cf70f98909e57534d87ec2b2e5 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 20 Nov 2014 21:35:49 +0100 Subject: [PATCH 59/86] [ardrone] Handle memory full FTP upload error on ardrone (#967) --- sw/ext/ardrone2_drivers/ardrone2.py | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/sw/ext/ardrone2_drivers/ardrone2.py b/sw/ext/ardrone2_drivers/ardrone2.py index 486f20cc2a4..e3c02430e94 100755 --- a/sw/ext/ardrone2_drivers/ardrone2.py +++ b/sw/ext/ardrone2_drivers/ardrone2.py @@ -6,8 +6,10 @@ import socket import telnetlib import os +import sys from time import sleep from ftplib import FTP +import ftplib # Check if IP is valid @@ -102,7 +104,7 @@ def ardrone2_reboot(): # Install the vision framework def ardrone2_install_vision(): print('Uploading GST') - ftp.storbinary("STOR arm_light.tgz", file("bin/arm_light.tgz", "rb")) + uploadfile(ftp, "arm_light.tgz", file("bin/arm_light.tgz", "rb")) print(execute_command("cd /data/video && tar -xzf arm_light.tgz")) print(execute_command("rm -rf /data/video/arm_light.tgz")) print('Now Starting Vision') @@ -134,14 +136,14 @@ def ardrone2_start_vision(): # Install autoboot script def ardrone2_install_autoboot(): print('Uploading autoboot script') - ftp.storbinary("STOR check_update.sh", file("check_update.sh", "rb")) + uploadfile(ftp, "check_update.sh", file("check_update.sh", "rb")) print(execute_command("mv /data/video/check_update.sh /bin/check_update.sh")) print(execute_command("chmod 777 /bin/check_update.sh")) # Install network script def ardrone2_install_network_script(): print('Uploading Wifi script') - ftp.storbinary("STOR wifi_setup.sh", file("wifi_setup.sh", "rb")) + uploadfile(ftp, "wifi_setup.sh", file("wifi_setup.sh", "rb")) print(execute_command("mv /data/video/wifi_setup.sh /bin/wifi_setup.sh")) print(execute_command("chmod 777 /bin/wifi_setup.sh")) @@ -203,6 +205,17 @@ def ardrone2_status(): print('\n======================== Filesystem Status ========================') print(check_filesystem()) +# Upload ftp and catch memory-full error +def uploadfile(ftp, filename, content): + try: + ftp.storbinary("STOR " + filename, content) + except ftplib.error_temp: + print("FTP UPLOAD ERROR: Uploading FAILED: Probably your ARDrone memory is full.") + sys.exit() + except: + print("FTP UPLOAD ERROR: Maybe your ARDrone memory is full?", sys.exc_info()[0]) + sys.exit() + # Parse the arguments parser = argparse.ArgumentParser(description='ARDrone 2 python helper. Use ardrone2.py -h for help') @@ -387,7 +400,7 @@ def ardrone2_status(): elif args.command == 'upload_gst_module': print('Uploading ...' + args.file) - ftp.storbinary("STOR " + args.file, file(args.file, "rb")) + uploadfile(ftp, args.file, file(args.file, "rb")) execute_command("chmod 777 /data/video/" + args.file) execute_command("mv /data/video/" + args.file + " /data/video/opt/arm/gst/lib/gstreamer-0.10") if check_vision_running(): @@ -407,7 +420,7 @@ def ardrone2_status(): elif args.command == 'insmod': modfile = split_into_path_and_file(args.file) print('Uploading \'' + modfile[1]) - ftp.storbinary("STOR " + modfile[1], file(args.file, "rb")) + uploadfile(ftp, modfile[1], file(args.file, "rb")) print(execute_command("insmod /data/video/" + modfile[1])) elif args.command == 'upload_file_and_run': @@ -419,7 +432,7 @@ def ardrone2_status(): sleep(1) execute_command("mkdir -p /data/video/" + args.folder) print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder) - ftp.storbinary("STOR " + args.folder + "/" + f[1], file(args.file, "rb")) + uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) sleep(0.5) execute_command("chmod 777 /data/video/" + args.folder + "/" + f[1]) execute_command("/data/video/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &") @@ -431,7 +444,7 @@ def ardrone2_status(): execute_command("mkdir -p /data/video/" + args.folder) print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder) - ftp.storbinary("STOR " + args.folder + "/" + f[1], file(args.file, "rb")) + uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) print("#pragma message: Upload of " + f[1] + " to ARDrone2 Succes!") elif args.command == 'download_file': From 2affa15bed1c3de6ffaf494960c79d2a2b8f6f3f Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sun, 23 Nov 2014 22:56:33 +0100 Subject: [PATCH 60/86] [uart] Fix stupid weak errors --- sw/airborne/arch/omap/mcu_periph/uart_arch.c | 59 +++++++++++++++++--- sw/airborne/mcu_periph/uart.c | 22 +------- sw/airborne/mcu_periph/uart.h | 9 +-- 3 files changed, 54 insertions(+), 36 deletions(-) diff --git a/sw/airborne/arch/omap/mcu_periph/uart_arch.c b/sw/airborne/arch/omap/mcu_periph/uart_arch.c index 1cfaf048983..b8d81f6519b 100644 --- a/sw/airborne/arch/omap/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/omap/mcu_periph/uart_arch.c @@ -36,6 +36,27 @@ // #define TRACE(fmt,args...) fprintf(stderr, fmt, args) #define TRACE(fmt,args...) +#if USE_UART0 +static inline void uart0_handler(void); +#endif +#if USE_UART1 +static inline void uart1_handler(void); +#endif +#if USE_UART2 +static inline void uart2_handler(void); +#endif +#if USE_UART3 +static inline void uart3_handler(void); +#endif +#if USE_UART4 +static inline void uart4_handler(void); +#endif +#if USE_UART5 +static inline void uart5_handler(void); +#endif +#if USE_UART6 +static inline void uart6_handler(void); +#endif void uart_periph_set_baudrate(struct uart_periph* periph, uint32_t baud) { struct SerialPort* port; @@ -117,6 +138,30 @@ static inline void uart_handler(struct uart_periph* periph) { } +void uart_event(void) { +#if USE_UART0 + uart0_handler(); +#endif +#if USE_UART1 + uart1_handler(); +#endif +#if USE_UART2 + uart2_handler(); +#endif +#if USE_UART3 + uart3_handler(); +#endif +#if USE_UART4 + uart4_handler(); +#endif +#if USE_UART5 + uart5_handler(); +#endif +#if USE_UART6 + uart6_handler(); +#endif +} + #if USE_UART0 void uart0_init( void ) { uart_periph_init(&uart0); @@ -124,7 +169,7 @@ void uart0_init( void ) { uart_periph_set_baudrate(&uart0, UART0_BAUD); } -void uart0_handler(void) { +static inline void uart0_handler(void) { uart_handler(&uart0); } #endif /* USE_UART0 */ @@ -136,7 +181,7 @@ void uart1_init( void ) { uart_periph_set_baudrate(&uart1, UART1_BAUD); } -void uart1_handler(void) { +static inline void uart1_handler(void) { uart_handler(&uart1); } #endif /* USE_UART1 */ @@ -148,7 +193,7 @@ void uart2_init(void) { uart_periph_set_baudrate(&uart2, UART2_BAUD); } -void uart2_handler(void) { +static inline void uart2_handler(void) { uart_handler(&uart2); } #endif /* USE_UART2 */ @@ -160,7 +205,7 @@ void uart3_init(void) { uart_periph_set_baudrate(&uart3, UART3_BAUD); } -void uart3_handler(void) { +static inline void uart3_handler(void) { uart_handler(&uart3); } #endif /* USE_UART3 */ @@ -172,7 +217,7 @@ void uart4_init(void) { uart_periph_set_baudrate(&uart4, UART4_BAUD); } -void uart4_handler(void) { +static inline void uart4_handler(void) { uart_handler(&uart4); } #endif /* USE_UART4 */ @@ -184,7 +229,7 @@ void uart5_init(void) { uart_periph_set_baudrate(&uart5, UART5_BAUD); } -void uart5_handler(void) { +static inline void uart5_handler(void) { uart_handler(&uart5); } #endif /* USE_UART5 */ @@ -196,7 +241,7 @@ void uart6_init(void) { uart_periph_set_baudrate(&uart6, UART6_BAUD); } -void uart6_handler(void) { +static inline void uart6_handler(void) { uart_handler(&uart6); } #endif /* USE_UART6 */ diff --git a/sw/airborne/mcu_periph/uart.c b/sw/airborne/mcu_periph/uart.c index 69a57179bd3..5246879e3db 100644 --- a/sw/airborne/mcu_periph/uart.c +++ b/sw/airborne/mcu_periph/uart.c @@ -214,25 +214,5 @@ uint8_t uart_getch(struct uart_periph* p) { } void WEAK uart_event(void) { -#if USE_UART0 - uart0_handler(); -#endif -#if USE_UART1 - uart1_handler(); -#endif -#if USE_UART2 - uart2_handler(); -#endif -#if USE_UART3 - uart3_handler(); -#endif -#if USE_UART4 - uart4_handler(); -#endif -#if USE_UART5 - uart5_handler(); -#endif -#if USE_UART6 - uart6_handler(); -#endif + } diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 64dfd879fb9..371b22babf2 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -82,7 +82,7 @@ extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_ extern void uart_transmit(struct uart_periph* p, uint8_t data); extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len); extern uint8_t uart_getch(struct uart_periph* p); -extern void WEAK uart_event(void); +extern void uart_event(void); /** * Check UART for available chars in receive buffer. @@ -99,7 +99,6 @@ static inline uint16_t uart_char_available(struct uart_periph* p) { #if USE_UART0 extern struct uart_periph uart0; extern void uart0_init(void); -extern void WEAK uart0_handler(void); #define UART0Init() uart_periph_init(&uart0) #define UART0CheckFreeSpace(_x) uart_check_free_space(&uart0, _x) @@ -116,7 +115,6 @@ extern void WEAK uart0_handler(void); #if USE_UART1 extern struct uart_periph uart1; extern void uart1_init(void); -extern void WEAK uart1_handler(void); #define UART1Init() uart_periph_init(&uart1) #define UART1CheckFreeSpace(_x) uart_check_free_space(&uart1, _x) @@ -133,7 +131,6 @@ extern void WEAK uart1_handler(void); #if USE_UART2 extern struct uart_periph uart2; extern void uart2_init(void); -extern void WEAK uart2_handler(void); #define UART2Init() uart_periph_init(&uart2) #define UART2CheckFreeSpace(_x) uart_check_free_space(&uart2, _x) @@ -150,7 +147,6 @@ extern void WEAK uart2_handler(void); #if USE_UART3 extern struct uart_periph uart3; extern void uart3_init(void); -extern void WEAK uart3_handler(void); #define UART3Init() uart_periph_init(&uart3) #define UART3CheckFreeSpace(_x) uart_check_free_space(&uart3, _x) @@ -167,7 +163,6 @@ extern void WEAK uart3_handler(void); #if USE_UART4 extern struct uart_periph uart4; extern void uart4_init(void); -extern void WEAK uart4_handler(void); #define UART4Init() uart_periph_init(&uart4) #define UART4CheckFreeSpace(_x) uart_check_free_space(&uart4, _x) @@ -184,7 +179,6 @@ extern void WEAK uart4_handler(void); #if USE_UART5 extern struct uart_periph uart5; extern void uart5_init(void); -extern void WEAK uart5_handler(void); #define UART5Init() uart_periph_init(&uart5) #define UART5CheckFreeSpace(_x) uart_check_free_space(&uart5, _x) @@ -201,7 +195,6 @@ extern void WEAK uart5_handler(void); #if USE_UART6 extern struct uart_periph uart6; extern void uart6_init(void); -extern void WEAK uart6_handler(void); #define UART6Init() uart_periph_init(&uart6) #define UART6CheckFreeSpace(_x) uart_check_free_space(&uart6, _x) From 3ae4c0a4ec985d4077d62b5ec996571d25c147e3 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Sun, 23 Nov 2014 22:57:03 +0100 Subject: [PATCH 61/86] [gps] Fix NMEA pointer error --- sw/airborne/subsystems/gps/gps_nmea.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/gps/gps_nmea.h b/sw/airborne/subsystems/gps/gps_nmea.h index cadece9d0b5..d416b34ad78 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.h +++ b/sw/airborne/subsystems/gps/gps_nmea.h @@ -88,13 +88,13 @@ extern struct GpsNmea gps_nmea; extern void nmea_parse_char(uint8_t c); extern void nmea_parse_msg(void); extern uint8_t nmea_calc_crc(const char *buff, int buff_sz); -void nmea_parse_prop_init(void); -void nmea_parse_prop_msg(void); +extern void nmea_parse_prop_init(void); +extern void nmea_parse_prop_msg(void); /** Read until a certain character, placed here for proprietary includes */ static inline void nmea_read_until(int *i) { - while (gps_nmea.msg_buf[*i++] != ',') { + while (gps_nmea.msg_buf[(*i)++] != ',') { if (*i >= gps_nmea.msg_len) { return; } From 1cf9b0e20661fa6a0696159b596f9e01c3fcc013 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sun, 23 Nov 2014 23:45:24 +0100 Subject: [PATCH 62/86] [gcs] also save size of widget when saving GCS layout close #968 --- sw/ground_segment/cockpit/gcs.ml | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 5a1e9986127..d017b37219e 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -547,6 +547,28 @@ let rec replace_widget_children = fun name children xml -> loop xmls) | _ -> xml +let rec update_widget_size = fun orientation widgets xml -> + let get_size = fun (widget:GObj.widget) orientation -> + let rect = widget#misc#allocation in + if orientation = `HORIZONTAL then rect.Gtk.width else rect.Gtk.height + in + let xmls = Xml.children xml + and tag = String.lowercase (Xml.tag xml) in + match tag with + "widget" -> + let name = ExtXml.attrib xml "name" in + let widget = + try List.assoc name widgets with + Not_found -> failwith (sprintf "Unknown widget: '%s'" name) + in + let size = get_size widget orientation in + let xml = ExtXml.subst_attrib "size" (string_of_int size) xml in + Xml.Element("widget", Xml.attribs xml, xmls) + | "rows" -> + Xml.Element("rows", Xml.attribs xml, List.map (update_widget_size `VERTICAL widgets) xmls) + | "columns" -> + Xml.Element("columns", Xml.attribs xml, List.map (update_widget_size `HORIZONTAL widgets) xmls) + | x -> failwith (sprintf "update_widget_size: %s" x) @@ -673,6 +695,7 @@ let () = let save_layout = fun () -> let the_new_layout = replace_widget_children "map2d" (Papgets.dump_store ()) the_layout in let width, height = Gdk.Drawable.get_size window#misc#window in + let the_new_layout = update_widget_size `HORIZONTAL widgets the_new_layout in let new_layout = Xml.Element ("layout", ["width", soi width; "height", soi height], [the_new_layout]) in save_layout layout_file (Xml.to_string_fmt new_layout) in From dd3bed7b2f58dc800d927dc4bc6933e883ffb01c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 15:14:40 +0100 Subject: [PATCH 63/86] update changelog --- CHANGELOG.md | 96 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 40f0eb01cf3..2fa641d2243 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,99 @@ +Paparazzi 5.3_devel +=================== + +currently ongoing development, changes so far (no particular order, nor complete) + +General +------- + +- Flight plans: option to `call` functions once without checking return value + [830] (https://github.com/paparazzi/paparazzi/pull/830) +- Paparazzi Center settings improvements + [#834] (https://github.com/paparazzi/paparazzi/pull/834) +- replay: ignore non-telemetry messages to remove warnings + [#894] (https://github.com/paparazzi/paparazzi/issues/894) +- maps: put google tiles in var/maps/Google instead of var/maps + [#902] (https://github.com/paparazzi/paparazzi/issues/902) +- Paparazzi Center: improve warning coloring + [#910] (https://github.com/paparazzi/paparazzi/issues/910) +- add INFO_MSG with printing to GCS console + [#929] (https://github.com/paparazzi/paparazzi/pull/929) +- Remove array delimiters on Ivy messages + [#942] (https://github.com/paparazzi/paparazzi/pull/942) +- improve test framework + [#933] (https://github.com/paparazzi/paparazzi/pull/933) + [#945] (https://github.com/paparazzi/paparazzi/pull/945) +- GCS: save size in layout + [#968] (https://github.com/paparazzi/paparazzi/issues/968) + +Simulation +---------- + +- OCaml sim: simulate sys_time + [#962] (https://github.com/paparazzi/paparazzi/issues/962) +- OCaml sim: use unconnected socket for flightgear viz + [#915] (https://github.com/paparazzi/paparazzi/issues/915) +- NPS: add commandline option to set time_factor +- radio_conrol spektrum for sim target + [#851] (https://github.com/paparazzi/paparazzi/pull/851) + +Airborne +-------- + +- cleanup math lib and convert lots of macros to functions + [#819] (https://github.com/paparazzi/paparazzi/pull/819) +- radio_control spektrum also usable for intermcu + [#847] (https://github.com/paparazzi/paparazzi/pull/847) +- Convert air_data subsystem to module with QNH and true airspeed support + [#853] (https://github.com/paparazzi/paparazzi/pull/853) +- add airspeed_ms45xx_i2c module + [#852] (https://github.com/paparazzi/paparazzi/pull/852) +- Replace telemetry macros with functions + [#931] (https://github.com/paparazzi/paparazzi/pull/931) +- Add Furuno NMEA based GPS + [#959] (https://github.com/paparazzi/paparazzi/pull/959) +- Driver for MPU9250 + [#953] (https://github.com/paparazzi/paparazzi/pull/953) +- Driver for AKM8963 magnetometer + [#947] (https://github.com/paparazzi/paparazzi/pull/947) +- Basic linux I2C driver + [#961] (https://github.com/paparazzi/paparazzi/pull/961) +- ARDrone2: Handle memory full FTP upload error + [#967] (https://github.com/paparazzi/paparazzi/issues/967) + + +Paparazzi 5.2.1_stable +====================== + +Maintenance release + +- build system: remove 'load' target as it is a builtin directive in Make 4.0 +- fix FlightGear visualization on 32bit systems +- flight plans: set primitive should not delay next stage + [#824] (https://github.com/paparazzi/paparazzi/pull/824) +- flight plans: fix return primitive +- generators: gen_airframe: don't force float if unit = code_unit +- sys_time: up to 16 sys_time timers by default (was 8) +- OCaml: fix Pprz.sprint_value for uint32, e.g. for NatNet + [#831] (https://github.com/paparazzi/paparazzi/issues/831) +- Rotorcraft: auto-enable UNLOCKED_HOME_MODE if HOME mode is used on RC + [#823] (https://github.com/paparazzi/paparazzi/issues/823) +- Rotorcraft: only go to HOME mode if in NAV + [#850] (https://github.com/paparazzi/paparazzi/issues/850) +- Fixedwing: If USE_BARO_BOARD: separate baro timer + Before baro_periodic was running at PERIODIC_FREQUENCY in sensors_task, + which is too fast for ms5611 if periodic freq is > 100Hz. +- INS alt_float: if USE_BARO_BOARD, dt is 1/BARO_PERIODIC_FREQUENCY + [#848] (https://github.com/paparazzi/paparazzi/pull/848) +- STM32: backport fix for using multiple ADs + [#822] (https://github.com/paparazzi/paparazzi/pull/822) +- LPC21: don't override T0CCR values when setting ppm and trig_ext +- IMU driver for Lisa/M/MX 2.1 + [#817] (https://github.com/paparazzi/paparazzi/pull/817) +- support for HBmini board + [#864] (https://github.com/paparazzi/paparazzi/pull/864) + + Paparazzi 5.2.0_stable ======================= From f7ae72dcb33b43ab6d44ed4bb02d07d4c74a7880 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 18 Nov 2014 18:52:28 +0100 Subject: [PATCH 64/86] [tests] skeleton for testing math lib with libtap using [libtap](https://github.com/zorgnax/libtap) to create a [TAP](http://testanything.org) (TestAnythingProtocol) producer. You can also run the test (if already compiled) with prove: ``` prove --exec '' tests/math/test_pprz_math.run ``` --- sw/airborne/math/Makefile | 2 +- tests/Makefile | 3 +- tests/math/.gitignore | 1 + tests/math/Makefile | 64 +++++++ tests/math/tap.c | 371 ++++++++++++++++++++++++++++++++++++ tests/math/tap.h | 116 +++++++++++ tests/math/test_pprz_math.c | 46 +++++ 7 files changed, 601 insertions(+), 2 deletions(-) create mode 100644 tests/math/.gitignore create mode 100644 tests/math/Makefile create mode 100644 tests/math/tap.c create mode 100644 tests/math/tap.h create mode 100644 tests/math/test_pprz_math.c diff --git a/sw/airborne/math/Makefile b/sw/airborne/math/Makefile index ab90c6a2090..11cba79adb2 100644 --- a/sw/airborne/math/Makefile +++ b/sw/airborne/math/Makefile @@ -31,7 +31,7 @@ all: @cat README shared_lib: $(OBJ) - $(CC) -shared -o $(BUILDDIR)/$(LIBNAME).so $(OBJ) + $(CC) -shared -o $(BUILDDIR)/$(LIBNAME).so $(OBJ) -lm install_shared_lib: shared_lib test -d $(LIB_INSTALLDIR) || mkdir -p $(LIB_INSTALLDIR) diff --git a/tests/Makefile b/tests/Makefile index 37c10dca345..3eb9e7fe3a2 100644 --- a/tests/Makefile +++ b/tests/Makefile @@ -15,7 +15,7 @@ else TEST_DIRECTORIES = $(NON_HARDWARE_TEST_DIRS) $(HARDWARE_TEST_DIRS) endif endif -TEST_FILES ?= $(shell ls $(TEST_DIRECTORIES:%=%/*.t)) +TEST_FILES ?= $(shell ls $(TEST_DIRECTORIES:%=%/*.t) 2> /dev/null) ifneq ($(JUNIT),) PERLENV=PERL_TEST_HARNESS_DUMP_TAP=$(PAPARAZZI_SRC)/tests/results @@ -26,6 +26,7 @@ else endif test: + $(Q)make -C math test $(Q)$(PERLENV) $(PERL) "-e" "$(RUNTESTS)" clean: diff --git a/tests/math/.gitignore b/tests/math/.gitignore new file mode 100644 index 00000000000..5ad50ca5138 --- /dev/null +++ b/tests/math/.gitignore @@ -0,0 +1 @@ +test_pprz_math.run diff --git a/tests/math/Makefile b/tests/math/Makefile new file mode 100644 index 00000000000..3806678baf1 --- /dev/null +++ b/tests/math/Makefile @@ -0,0 +1,64 @@ +# Copyright (C) 2014 Piotr Esden-Tempski +# +# This file is part of paparazzi. +# +# paparazzi is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2, or (at your option) +# any later version. +# +# paparazzi is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with paparazzi; see the file COPYING. If not, see +# . + +# The default is to produce a quiet echo of compilation commands +# Launch with "make Q=''" to get full echo + +# Make sure all our environment is set properly in case we run make not from toplevel director. +Q ?= @ + +PAPARAZZI_SRC ?= $(shell pwd)/../.. +ifeq ($(PAPARAZZI_HOME),) +PAPARAZZI_HOME=$(PAPARAZZI_SRC) +endif + +# export the PAPARAZZI environment to sub-make +export PAPARAZZI_SRC +export PAPARAZZI_HOME + +MATHSRC_PATH=$(PAPARAZZI_SRC)/sw/airborne/math +MATHLIB_PATH=$(PAPARAZZI_SRC)/var/build/math + +##################################################### +# If you add more test files you add their names here +TESTS = test_pprz_math.run + +################################################### +# You should not need to touch the rest of the file + +all: test + +math_shlib: + $(Q)cd $(MATHSRC_PATH); make shared_lib + +build_tests: math_shlib $(TESTS) + +test: build_tests + $(Q)for i in $(TESTS); do \ + ./$$i; \ + done + +%.run: %.c + $(CC) -L$(MATHLIB_PATH) -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/sw/include tap.c $< -lpprzmath -o $@ + +clean: + $(Q)rm -f $(MATHLIB_PATH)/*.o $(MATHLIB_PATH)/libpprzmath.so + $(Q)rm -f $(TESTS) + + +.PHONY: math_shlib build_tests test clean all diff --git a/tests/math/tap.c b/tests/math/tap.c new file mode 100644 index 00000000000..24de64eab0b --- /dev/null +++ b/tests/math/tap.c @@ -0,0 +1,371 @@ +/* +libtap - Write tests in C +Copyright 2012 Jake Gelbman +This file is licensed under the GPLv2 or any later version +*/ + +#define _BSD_SOURCE 1 + +#include +#include +#include +#include +#include "tap.h" + +static int expected_tests = NO_PLAN; +static int failed_tests; +static int current_test; +static char *todo_mesg; + +static char * +vstrdupf (const char *fmt, va_list args) { + char *str; + int size; + va_list args2; + va_copy(args2, args); + if (!fmt) + fmt = ""; + size = vsnprintf(NULL, 0, fmt, args2) + 2; + str = malloc(size); + if (!str) { + perror("malloc error"); + exit(1); + } + vsprintf(str, fmt, args); + va_end(args2); + return str; +} + +void +tap_plan (int tests, const char *fmt, ...) { + expected_tests = tests; + if (tests == SKIP_ALL) { + char *why; + va_list args; + va_start(args, fmt); + why = vstrdupf(fmt, args); + va_end(args); + printf("1..0 "); + note("SKIP %s\n", why); + exit(0); + } + if (tests != NO_PLAN) { + printf("1..%d\n", tests); + } +} + +int +vok_at_loc (const char *file, int line, int test, const char *fmt, + va_list args) +{ + char *name = vstrdupf(fmt, args); + if (!test) + printf("not "); + printf("ok %d", ++current_test); + if (*name) + printf(" - %s", name); + if (todo_mesg) { + printf(" # TODO"); + if (*todo_mesg) + printf(" %s", todo_mesg); + } + printf("\n"); + if (!test) { + fprintf(stderr, "# Failed "); + if (todo_mesg) + fprintf(stderr, "(TODO) "); + fprintf(stderr, "test "); + if (*name) + fprintf(stderr, "'%s'\n# ", name); + fprintf(stderr, "at %s line %d.\n", file, line); + if (!todo_mesg) + failed_tests++; + } + free(name); + return test; +} + +int +ok_at_loc (const char *file, int line, int test, const char *fmt, ...) { + va_list args; + va_start(args, fmt); + vok_at_loc(file, line, test, fmt, args); + va_end(args); + return test; +} + +static int +mystrcmp (const char *a, const char *b) { + return a == b ? 0 : !a ? -1 : !b ? 1 : strcmp(a, b); +} + +#define eq(a, b) (!mystrcmp(a, b)) +#define ne(a, b) (mystrcmp(a, b)) + +int +is_at_loc (const char *file, int line, const char *got, const char *expected, + const char *fmt, ...) +{ + int test = eq(got, expected); + va_list args; + va_start(args, fmt); + vok_at_loc(file, line, test, fmt, args); + va_end(args); + if (!test) { + diag(" got: '%s'", got); + diag(" expected: '%s'", expected); + } + return test; +} + +int +isnt_at_loc (const char *file, int line, const char *got, const char *expected, + const char *fmt, ...) +{ + int test = ne(got, expected); + va_list args; + va_start(args, fmt); + vok_at_loc(file, line, test, fmt, args); + va_end(args); + if (!test) { + diag(" got: '%s'", got); + diag(" expected: anything else"); + } + return test; +} + +int +cmp_ok_at_loc (const char *file, int line, int a, const char *op, int b, + const char *fmt, ...) +{ + int test = eq(op, "||") ? a || b + : eq(op, "&&") ? a && b + : eq(op, "|") ? a | b + : eq(op, "^") ? a ^ b + : eq(op, "&") ? a & b + : eq(op, "==") ? a == b + : eq(op, "!=") ? a != b + : eq(op, "<") ? a < b + : eq(op, ">") ? a > b + : eq(op, "<=") ? a <= b + : eq(op, ">=") ? a >= b + : eq(op, "<<") ? a << b + : eq(op, ">>") ? a >> b + : eq(op, "+") ? a + b + : eq(op, "-") ? a - b + : eq(op, "*") ? a * b + : eq(op, "/") ? a / b + : eq(op, "%") ? a % b + : diag("unrecognized operator '%s'", op); + va_list args; + va_start(args, fmt); + vok_at_loc(file, line, test, fmt, args); + va_end(args); + if (!test) { + diag(" %d", a); + diag(" %s", op); + diag(" %d", b); + } + return test; +} + +static int +find_mem_diff (const char *a, const char *b, size_t n, size_t *offset) { + size_t i; + if (a == b) + return 0; + if (!a || !b) + return 2; + for (i = 0; i < n; i++) { + if (a[i] != b[i]) { + *offset = i; + return 1; + } + } + return 0; +} + +int +cmp_mem_at_loc (const char *file, int line, const void *got, + const void *expected, size_t n, const char *fmt, ...) +{ + size_t offset; + int diff = find_mem_diff(got, expected, n, &offset); + va_list args; + va_start(args, fmt); + vok_at_loc(file, line, !diff, fmt, args); + va_end(args); + if (diff == 1) { + diag(" Difference starts at offset %d", offset); + diag(" got: 0x%02x", ((unsigned char *)got)[offset]); + diag(" expected: 0x%02x", ((unsigned char *)expected)[offset]); + } + else if (diff == 2) { + diag(" got: %s", got ? "not NULL" : "NULL"); + diag(" expected: %s", expected ? "not NULL" : "NULL"); + } + return !diff; +} + +static void +vdiag_to_fh (FILE *fh, const char *fmt, va_list args) { + char *mesg, *line; + int i; + if (!fmt) + return; + mesg = vstrdupf(fmt, args); + line = mesg; + for (i = 0; *line; i++) { + char c = mesg[i]; + if (!c || c == '\n') { + mesg[i] = '\0'; + fprintf(fh, "# %s\n", line); + if (!c) + break; + mesg[i] = c; + line = mesg + i + 1; + } + } + free(mesg); + return; +} + +int +diag (const char *fmt, ...) { + va_list args; + va_start(args, fmt); + vdiag_to_fh(stderr, fmt, args); + va_end(args); + return 0; +} + +int +note (const char *fmt, ...) { + va_list args; + va_start(args, fmt); + vdiag_to_fh(stdout, fmt, args); + va_end(args); + return 0; +} + +int +exit_status () { + int retval = 0; + if (expected_tests == NO_PLAN) { + printf("1..%d\n", current_test); + } + else if (current_test != expected_tests) { + diag("Looks like you planned %d test%s but ran %d.", + expected_tests, expected_tests > 1 ? "s" : "", current_test); + retval = 255; + } + if (failed_tests) { + diag("Looks like you failed %d test%s of %d run.", + failed_tests, failed_tests > 1 ? "s" : "", current_test); + if (expected_tests == NO_PLAN) + retval = failed_tests; + else + retval = expected_tests - current_test + failed_tests; + } + return retval; +} + +int +bail_out (int ignore, const char *fmt, ...) { + va_list args; + va_start(args, fmt); + printf("Bail out! "); + vprintf(fmt, args); + printf("\n"); + va_end(args); + exit(255); + return 0; +} + +void +tap_skip (int n, const char *fmt, ...) { + char *why; + va_list args; + va_start(args, fmt); + why = vstrdupf(fmt, args); + va_end(args); + while (n --> 0) { + printf("ok %d ", ++current_test); + note("skip %s\n", why); + } + free(why); +} + +void +tap_todo (int ignore, const char *fmt, ...) { + va_list args; + va_start(args, fmt); + todo_mesg = vstrdupf(fmt, args); + va_end(args); +} + +void +tap_end_todo () { + free(todo_mesg); + todo_mesg = NULL; +} + +#ifndef _WIN32 +#include +#include +#include + +#if defined __APPLE__ || defined BSD +#define MAP_ANONYMOUS MAP_ANON +#endif + +/* Create a shared memory int to keep track of whether a piece of code executed +dies. to be used in the dies_ok and lives_ok macros. */ +int +tap_test_died (int status) { + static int *test_died = NULL; + int prev; + if (!test_died) { + test_died = mmap(0, sizeof (int), PROT_READ | PROT_WRITE, + MAP_SHARED | MAP_ANONYMOUS, -1, 0); + *test_died = 0; + } + prev = *test_died; + *test_died = status; + return prev; +} + +int +like_at_loc (int for_match, const char *file, int line, const char *got, + const char *expected, const char *fmt, ...) +{ + int test; + regex_t re; + va_list args; + int err = regcomp(&re, expected, REG_EXTENDED); + if (err) { + char errbuf[256]; + regerror(err, &re, errbuf, sizeof errbuf); + fprintf(stderr, "Unable to compile regex '%s': %s at %s line %d\n", + expected, errbuf, file, line); + exit(255); + } + err = regexec(&re, got, 0, NULL, 0); + regfree(&re); + test = for_match ? !err : err; + va_start(args, fmt); + vok_at_loc(file, line, test, fmt, args); + va_end(args); + if (!test) { + if (for_match) { + diag(" '%s'", got); + diag(" doesn't match: '%s'", expected); + } + else { + diag(" '%s'", got); + diag(" matches: '%s'", expected); + } + } + return test; +} +#endif diff --git a/tests/math/tap.h b/tests/math/tap.h new file mode 100644 index 00000000000..e2caef60204 --- /dev/null +++ b/tests/math/tap.h @@ -0,0 +1,116 @@ +/* +libtap - Write tests in C +Copyright 2012 Jake Gelbman +This file is licensed under the GPLv2 or any later version +*/ + +#ifndef __TAP_H__ +#define __TAP_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef va_copy +#ifdef __va_copy +#define va_copy __va_copy +#else +#define va_copy(d, s) ((d) = (s)) +#endif +#endif + +#include +#include +#include + +int vok_at_loc (const char *file, int line, int test, const char *fmt, + va_list args); +int ok_at_loc (const char *file, int line, int test, const char *fmt, + ...); +int is_at_loc (const char *file, int line, const char *got, + const char *expected, const char *fmt, ...); +int isnt_at_loc (const char *file, int line, const char *got, + const char *expected, const char *fmt, ...); +int cmp_ok_at_loc (const char *file, int line, int a, const char *op, + int b, const char *fmt, ...); +int cmp_mem_at_loc (const char *file, int line, const void *got, + const void *expected, size_t n, const char *fmt, ...); +int bail_out (int ignore, const char *fmt, ...); +void tap_plan (int tests, const char *fmt, ...); +int diag (const char *fmt, ...); +int note (const char *fmt, ...); +int exit_status (void); +void tap_skip (int n, const char *fmt, ...); +void tap_todo (int ignore, const char *fmt, ...); +void tap_end_todo (void); + +#define NO_PLAN -1 +#define SKIP_ALL -2 +#define ok(...) ok_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL) +#define is(...) is_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL) +#define isnt(...) isnt_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL) +#define cmp_ok(...) cmp_ok_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL) +#define cmp_mem(...) cmp_mem_at_loc(__FILE__, __LINE__, __VA_ARGS__, NULL); +#define plan(...) tap_plan(__VA_ARGS__, NULL) +#define done_testing() return exit_status() +#define BAIL_OUT(...) bail_out(0, "" __VA_ARGS__, NULL) +#define pass(...) ok(1, "" __VA_ARGS__) +#define fail(...) ok(0, "" __VA_ARGS__) + +#define skip(test, ...) do {if (test) {tap_skip(__VA_ARGS__, NULL); break;} +#define end_skip } while (0) + +#define todo(...) tap_todo(0, "" __VA_ARGS__, NULL) +#define end_todo tap_end_todo() + +#define dies_ok(...) dies_ok_common(1, __VA_ARGS__) +#define lives_ok(...) dies_ok_common(0, __VA_ARGS__) + +#ifdef _WIN32 +#define like(...) tap_skip(1, "like is not implemented on Windows") +#define unlike tap_skip(1, "unlike is not implemented on Windows") +#define dies_ok_common(...) \ + tap_skip(1, "Death detection is not supported on Windows") +#else +#define like(...) like_at_loc(1, __FILE__, __LINE__, __VA_ARGS__, NULL) +#define unlike(...) like_at_loc(0, __FILE__, __LINE__, __VA_ARGS__, NULL) +int like_at_loc (int for_match, const char *file, int line, + const char *got, const char *expected, + const char *fmt, ...); +#include +#include +#include +int tap_test_died (int status); +#define dies_ok_common(for_death, code, ...) \ + do { \ + int cpid; \ + int it_died; \ + tap_test_died(1); \ + cpid = fork(); \ + switch (cpid) { \ + case -1: \ + perror("fork error"); \ + exit(1); \ + case 0: \ + close(1); \ + close(2); \ + code \ + tap_test_died(0); \ + exit(0); \ + } \ + if (waitpid(cpid, NULL, 0) < 0) { \ + perror("waitpid error"); \ + exit(1); \ + } \ + it_died = tap_test_died(0); \ + if (!it_died) \ + {code} \ + ok(for_death ? it_died : !it_died, "" __VA_ARGS__); \ + } while (0) +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/tests/math/test_pprz_math.c b/tests/math/test_pprz_math.c new file mode 100644 index 00000000000..f41db4febda --- /dev/null +++ b/tests/math/test_pprz_math.c @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2014 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file test_pprz_math.c + * @brief Tests for Paparazzi math libary. + * + * Using libtap to create a TAP (TestAnythingProtocol) producer: + * https://github.com/zorgnax/libtap + * + */ + +#include "tap.h" +#include "math/pprz_algebra_int.h" + +int main() +{ + plan(1); + + /* test int32_vect2_normalize */ + struct Int32Vect2 v = {2300, -4200}; + int32_vect2_normalize(&v, 10); + + ok((v.x == 491 && v.y == -898), + "int32_vect2_normalize([2300, -4200], 10) returned [%d, %d]", v.x, v.y); + + + done_testing(); +} From da5de5445092f7eafda2754f94c21cb9433472f2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 18:55:52 +0100 Subject: [PATCH 65/86] [tests] add libpprzmath.so to LD_LIBRARY_PATH for running tests --- tests/math/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/math/Makefile b/tests/math/Makefile index 3806678baf1..102adeee0a4 100644 --- a/tests/math/Makefile +++ b/tests/math/Makefile @@ -50,7 +50,7 @@ build_tests: math_shlib $(TESTS) test: build_tests $(Q)for i in $(TESTS); do \ - ./$$i; \ + LD_LIBRARY_PATH=$(MATHLIB_PATH):$LD_LIBRARY_PATH ./$$i; \ done %.run: %.c From 0bafabcc0f52bbc2682551a7f124dc407a39cd30 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 18:58:39 +0100 Subject: [PATCH 66/86] [travis] run math tests --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 5c1ac90784f..8d74e5b9c51 100644 --- a/.travis.yml +++ b/.travis.yml @@ -11,6 +11,7 @@ install: before_script: cd conf && ln -s conf_tests.xml conf.xml && cd .. script: - make + - make -C tests/math test - PAPARAZZI_SRC=$PWD PAPARAZZI_HOME=$PWD J=AUTO prove tests/examples/ notifications: From 63ba5a17a3cccf9a6645fbbcc15b4d00554d3b6b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 20:01:46 +0100 Subject: [PATCH 67/86] [rotorcraft] force MODE_STARTUP instead of KILL until ahrs is aligned - Go to MODE_STARTUP (which defaults to AP_MODE_KILL) instead of AP_MODE_KILL as long as the ahrs is not aligned. - Remove AUTOPILOT_DISABLE_AHRS_KILL from all example airframes (but kept the functionality for now). MODE_STARTUP is set to AP_MODE_NAV for the autonomous (without RC) ARDrone2. Without AUTOPILOT_DISABLE_AHRS_KILL it will force MODE_STARTUP until the ahrs is aligned, but still not allow to turn on the motors (as that also checks for ahrs_is_aligned). As far as I can see this should solve #964 as long as you don't have AUTOPILOT_DISABLE_AHRS_KILL defined. --- conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml | 1 - conf/airframes/TUDelft/silverlit_lisas.xml | 1 - conf/airframes/ardrone2_raw.xml | 1 - conf/airframes/ardrone2_raw_optitrack.xml | 1 - conf/airframes/examples/quadrotor_lisa_s.xml | 1 - conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile | 2 +- sw/airborne/firmwares/rotorcraft/autopilot.c | 4 ++-- 7 files changed, 3 insertions(+), 8 deletions(-) diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index 61ca12c6041..6ea8ec296a8 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -7,7 +7,6 @@ - diff --git a/conf/airframes/TUDelft/silverlit_lisas.xml b/conf/airframes/TUDelft/silverlit_lisas.xml index df002cedc5e..0190718f918 100644 --- a/conf/airframes/TUDelft/silverlit_lisas.xml +++ b/conf/airframes/TUDelft/silverlit_lisas.xml @@ -29,7 +29,6 @@ Lisa/S v1.0 board (http://wiki.paparazziuav.org/wiki/Lisa/S) - diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index caa8ac1befc..54819657c82 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -4,7 +4,6 @@ - diff --git a/conf/airframes/ardrone2_raw_optitrack.xml b/conf/airframes/ardrone2_raw_optitrack.xml index fb95f4f5a31..4699285bb0a 100644 --- a/conf/airframes/ardrone2_raw_optitrack.xml +++ b/conf/airframes/ardrone2_raw_optitrack.xml @@ -4,7 +4,6 @@ - diff --git a/conf/airframes/examples/quadrotor_lisa_s.xml b/conf/airframes/examples/quadrotor_lisa_s.xml index 0d0f2fce634..aad4599cec2 100644 --- a/conf/airframes/examples/quadrotor_lisa_s.xml +++ b/conf/airframes/examples/quadrotor_lisa_s.xml @@ -215,7 +215,6 @@ - diff --git a/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile b/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile index 759efc1d21e..1185c1c4047 100644 --- a/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile +++ b/conf/firmwares/subsystems/rotorcraft/ins_ardrone2.makefile @@ -2,6 +2,6 @@ # simple INS with float vertical filter # -$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_ardrone2.h\" -DAUTOPILOT_DISABLE_AHRS_KILL +$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_ardrone2.h\" $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_ardrone2.c diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 06e081532b6..3d8b77a4920 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -347,9 +347,9 @@ INFO("Using FAILSAFE_GROUND_DETECT: KILL") void autopilot_set_mode(uint8_t new_autopilot_mode) { - /* force kill mode as long as AHRS is not aligned */ + /* force startup mode (default is kill) as long as AHRS is not aligned */ if (!ahrs_is_aligned()) - new_autopilot_mode = AP_MODE_KILL; + new_autopilot_mode = MODE_STARTUP; if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ From 78b52b6f3c3538cabb659886c85f391d4411e537 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 24 Nov 2014 21:14:50 +0100 Subject: [PATCH 68/86] [rotorcraft] arming: only check if motors can be armed if ahrs_is_aligned --- sw/airborne/firmwares/rotorcraft/autopilot.c | 11 ++++++++--- .../firmwares/rotorcraft/autopilot_arming_throttle.h | 7 +------ .../firmwares/rotorcraft/autopilot_arming_yaw.h | 5 +---- 3 files changed, 10 insertions(+), 13 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 3d8b77a4920..5924ab731d5 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -527,9 +527,14 @@ void autopilot_on_rc_frame(void) { } #endif - /* an arming sequence is used to start/stop motors */ - autopilot_arming_check_motors_on(); - kill_throttle = ! autopilot_motors_on; + /* an arming sequence is used to start/stop motors. + * only allow switching motor if not in FAILSAFE or KILL mode and ahrs is aligned + */ + if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE && + ahrs_is_aligned()) { + autopilot_arming_check_motors_on(); + kill_throttle = ! autopilot_motors_on; + } guidance_v_read_rc(); guidance_h_read_rc(autopilot_in_flight); diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h index b58cd6ad0fb..8ecfdde3a9b 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h @@ -69,14 +69,10 @@ static inline void autopilot_arming_set(bool_t motors_on) { * - if throttle was not down at startup, you need to put throttle down again first * - other sticks need to be centered to start motors * - need to be in manual mode to start the motors - * - AHRS needs to be aligned */ static inline void autopilot_arming_check_motors_on( void ) { - /* only allow switching motor if not in FAILSAFE or KILL mode */ - if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE) { - - switch(autopilot_arming_state) { + switch(autopilot_arming_state) { case STATE_UNINIT: autopilot_motors_on = FALSE; autopilot_arming_delay_counter = 0; @@ -140,7 +136,6 @@ static inline void autopilot_arming_check_motors_on( void ) { break; default: break; - } } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h index 51d23007b34..9307ad3d24c 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h @@ -75,10 +75,8 @@ static inline void autopilot_arming_set(bool_t motors_on) { * The stick must return to a neutral position before starting/stoping again. */ static inline void autopilot_arming_check_motors_on( void ) { - /* only allow switching motor if not in FAILSAFE or KILL mode */ - if (autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_FAILSAFE) { - switch(autopilot_check_motor_status) { + switch(autopilot_check_motor_status) { case STATUS_MOTORS_OFF: autopilot_motors_on = FALSE; autopilot_motors_on_counter = 0; @@ -121,7 +119,6 @@ static inline void autopilot_arming_check_motors_on( void ) { break; default: break; - }; } } From 0cd9ed4cc6a4aba2d8a32af69e9674ab89a91852 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 21 Nov 2014 20:11:51 +0100 Subject: [PATCH 69/86] [omap] start basic spi driver totally untested... --- sw/airborne/arch/omap/mcu_periph/spi_arch.c | 180 ++++++++++++++++++++ sw/airborne/arch/omap/mcu_periph/spi_arch.h | 30 ++++ 2 files changed, 210 insertions(+) create mode 100644 sw/airborne/arch/omap/mcu_periph/spi_arch.c create mode 100644 sw/airborne/arch/omap/mcu_periph/spi_arch.h diff --git a/sw/airborne/arch/omap/mcu_periph/spi_arch.c b/sw/airborne/arch/omap/mcu_periph/spi_arch.c new file mode 100644 index 00000000000..eb5783482e7 --- /dev/null +++ b/sw/airborne/arch/omap/mcu_periph/spi_arch.c @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2014 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file arch/omap/mcu_periph/spi_arch.c + * + */ + +#include +#include +#include +#include +#include + +#include +#include + +#include "mcu_periph/spi.h" + + +void spi_init_slaves(void) +{ + /* for now we assume that each SPI device has it's SLAVE CS already set up + * e.g. in pin muxing of BBB + */ +} + +bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) +{ + int fd = (int)p->reg_addr; + + struct spi_ioc_transfer xfer; + memset(&xfer, 0, sizeof xfer); + + /* length in bytes of transaction */ + uint8_t buf_len = Max(t->input_length, t->output_length); + + /* handle transactions with different input/output length */ + if (buf_len > t->output_length) { + uint8_t tx_buf[buf_len]; + memset(tx_buf, 0, sizeof tx_buf); + /* copy bytes to transmit to larger buffer, rest filled with zero */ + memcpy(tx_buf, (void*)t->output_buf, t->output_length); + xfer.tx_buf = (unsigned long)tx_buf; + } + else { + xfer.tx_buf = (unsigned long)t->output_buf; + } + + if (buf_len > t->input_length) { + uint8_t rx_buf[buf_len]; + memset(rx_buf, 0, sizeof rx_buf); + xfer.rx_buf = (unsigned long)rx_buf; + } + else { + xfer.rx_buf = (unsigned long)t->input_buf; + } + + xfer.len = buf_len; + /* fixed speed of 1Mhz for now, use SPIClockDiv?? */ + xfer.speed_hz = 1000000; + xfer.delay_usecs = 0; + if (t->dss == SPIDss16bit) { + xfer.bits_per_word = 16; + } else { + xfer.bits_per_word = 8; + } + if (t->select == SPISelectUnselect || t->select == SPIUnselect) { + xfer.cs_change = 1; + } + + if (ioctl(fd, SPI_IOC_MESSAGE(1), xfer) < 0) { + t->status = SPITransFailed; + return FALSE; + } + + /* copy recieved data if we had to use an extra rx_buffer */ + if (buf_len > t->input_length) { + memcpy((void*)t->input_buf, (void*)xfer.rx_buf, t->input_length); + } + + t->status = SPITransSuccess; + return TRUE; +} + +bool_t spi_lock(struct spi_periph* p, uint8_t slave) { + // not implemented + return FALSE; +} + +bool_t spi_resume(struct spi_periph* p, uint8_t slave) { + // not implemented + return FALSE; +} + + +#if USE_SPI0 +void spi0_arch_init(void) +{ + int fd = open("/dev/spidev1.0", O_RDWR); + + if (fd < 0) { + perror("Could not open SPI device /dev/spidev1.0"); + spi0.reg_addr = NULL; + return; + } + spi0.reg_addr = (void*)fd; + + /* spi mode */ + if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL|SPI_CPHA)) < 0) { + perror("SPI0: can't set spi mode"); + } + + /* set to MSB first */ + if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, 0) < 0) { + perror("SPI0: can't set spi bit justification"); + } + + /* bits per word default to 8 */ + if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, 8) < 0) { + perror("SPI0: can't set bits per word"); + } + + /* max speed in hz, 1MHz for now */ + if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, 1000000) < 0) { + perror("SPI0: can't set max speed hz"); + } +} +#endif /* USE_SPI0 */ + +#if USE_SPI1 +void spi1_arch_init(void) +{ + int fd = open("/dev/spidev1.1", O_RDWR); + + if (fd < 0) { + perror("Could not open SPI device /dev/spidev1.1"); + spi1.reg_addr = NULL; + return; + } + spi1.reg_addr = (void*)fd; + + /* spi mode */ + if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL|SPI_CPHA)) < 0) { + perror("SPI1: can't set spi mode"); + } + + /* set to MSB first */ + if (ioctl(fd, SPI_IOC_WR_LSB_FIRST, 0) < 0) { + perror("SPI1: can't set spi bit justification"); + } + + /* bits per word default to 8 */ + if (ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, 8) < 0) { + perror("SPI1: can't set bits per word"); + } + + /* max speed in hz, 1MHz for now */ + if (ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, 1000000) < 0) { + perror("SPI1: can't set max speed hz"); + } +} +#endif /* USE_SPI1 */ diff --git a/sw/airborne/arch/omap/mcu_periph/spi_arch.h b/sw/airborne/arch/omap/mcu_periph/spi_arch.h new file mode 100644 index 00000000000..f5d8d53eb61 --- /dev/null +++ b/sw/airborne/arch/omap/mcu_periph/spi_arch.h @@ -0,0 +1,30 @@ +/* + * Copyright (C) 2014 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file arch/omap/mcu_periph/spi_arch.h + * Handling of SPI hardware for Linux/OMAP. + */ + +#ifndef SPI_ARCH_H +#define SPI_ARCH_H + + +#endif // SPI_ARCH_H From 4f39d2ab7efff136e40b24cca1896c2ff3b5123e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 25 Nov 2014 14:30:57 +0100 Subject: [PATCH 70/86] [modules] rotorcraft_cam: fix DEFAULT_MODE --- conf/airframes/ENAC/quadrotor/blender.xml | 2 +- conf/airframes/examples/quadrotor_navgo.xml | 82 +++++++++---------- .../modules/cam_control/rotorcraft_cam.c | 2 +- .../modules/cam_control/rotorcraft_cam.h | 1 - 4 files changed, 43 insertions(+), 44 deletions(-) diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 81d6606772e..dcc2e00f7ed 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -236,7 +236,7 @@
- +
diff --git a/conf/airframes/examples/quadrotor_navgo.xml b/conf/airframes/examples/quadrotor_navgo.xml index 1c8a2825a35..cbac95ce944 100644 --- a/conf/airframes/examples/quadrotor_navgo.xml +++ b/conf/airframes/examples/quadrotor_navgo.xml @@ -196,46 +196,46 @@
- - - - - - - - - - + + + + + + + + + +
-
- - - - - - - -
- -
- -
- -
- - - - -
- -
- -
- -
- -
+
+ + + + + + + +
+ +
+ +
+ +
+ + + + +
+ +
+ +
+ +
+ +
@@ -245,9 +245,9 @@
-
- -
+
+ +
diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index a801d691981..0fe477a7825 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -109,7 +109,7 @@ void rotorcraft_cam_init(void) { #ifdef ROTORCRAFT_CAM_SWITCH_GPIO gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO); #endif - rotorcraft_cam_SetCamMode(ROTORCRAFT_CAM_DEFAULT_MODE); + rotorcraft_cam_set_mode(ROTORCRAFT_CAM_DEFAULT_MODE); #if ROTORCRAFT_CAM_USE_TILT rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL; ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm); diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h index 0fb3c422c22..c11044289b3 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.h +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h @@ -42,7 +42,6 @@ #include "std.h" #include "generated/airframe.h" -#include "generated/flight_plan.h" #include "math/pprz_algebra_int.h" #include "mcu_periph/gpio.h" From 53d7f27fe057b6d1d268d1952996a07d74d44358 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 25 Nov 2014 15:03:34 +0100 Subject: [PATCH 71/86] [rotorcraft] navigation: fix NavCircleCount --- sw/airborne/firmwares/rotorcraft/navigation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 89f74dffa06..3712db21e9e 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -135,7 +135,7 @@ extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius); nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \ } -#define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI) +#define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI) #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; }) /** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/ From ee301947b6a2741c6b35150af21e4639568cc264 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 11:38:40 +0100 Subject: [PATCH 72/86] [sim] remove launchsitl symlink Please use pprzsim-launch directly, the launchsitl symlink was only meant to provide (partial) backwards compatibility for some time. closes #919 --- sw/simulator/launchsitl | 1 - 1 file changed, 1 deletion(-) delete mode 120000 sw/simulator/launchsitl diff --git a/sw/simulator/launchsitl b/sw/simulator/launchsitl deleted file mode 120000 index 9a485e824be..00000000000 --- a/sw/simulator/launchsitl +++ /dev/null @@ -1 +0,0 @@ -pprzsim-launch \ No newline at end of file From 1e2130271f21e16b15ad8c50d5cec32ea621ca63 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 13:07:02 +0100 Subject: [PATCH 73/86] [modules] rename flight_time to time_countdown closes #886 --- .../TUDelft/IMAV2013/ardrone2_raw.xml | 2 +- .../rotorcraft_basic_superbitrf.xml | 2 +- conf/modules/flight_time.xml | 25 ---------------- conf/modules/time_countdown.xml | 28 ++++++++++++++++++ .../time/{flight_time.c => time_countdown.c} | 29 ++++++++++++------- .../time/{flight_time.h => time_countdown.h} | 19 +++++++----- 6 files changed, 59 insertions(+), 46 deletions(-) delete mode 100644 conf/modules/flight_time.xml create mode 100644 conf/modules/time_countdown.xml rename sw/airborne/modules/time/{flight_time.c => time_countdown.c} (59%) rename sw/airborne/modules/time/{flight_time.h => time_countdown.h} (65%) diff --git a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml index 61ca12c6041..2632ca0cd99 100644 --- a/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml +++ b/conf/airframes/TUDelft/IMAV2013/ardrone2_raw.xml @@ -33,7 +33,7 @@ - + diff --git a/conf/flight_plans/rotorcraft_basic_superbitrf.xml b/conf/flight_plans/rotorcraft_basic_superbitrf.xml index aae3d4b9ce9..a73feb3014b 100644 --- a/conf/flight_plans/rotorcraft_basic_superbitrf.xml +++ b/conf/flight_plans/rotorcraft_basic_superbitrf.xml @@ -43,7 +43,7 @@ - + diff --git a/conf/modules/flight_time.xml b/conf/modules/flight_time.xml deleted file mode 100644 index fd614689453..00000000000 --- a/conf/modules/flight_time.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - Flight time calculator. - Allows to check how much time is left before the end of the competition. - - - - - - - - - -
- -
- - - - - - diff --git a/conf/modules/time_countdown.xml b/conf/modules/time_countdown.xml new file mode 100644 index 00000000000..4d1f756f300 --- /dev/null +++ b/conf/modules/time_countdown.xml @@ -0,0 +1,28 @@ + + + + + + + Count down time. + Set an initial countdown value in seconds (re-settable via settings/GCS) and countdown @a time_until_end variable to zero. + E.g. allows to check how much time is left before the end of the competition. + + + + + + + + + + +
+ +
+ + + + + + diff --git a/sw/airborne/modules/time/flight_time.c b/sw/airborne/modules/time/time_countdown.c similarity index 59% rename from sw/airborne/modules/time/flight_time.c rename to sw/airborne/modules/time/time_countdown.c index 16328c7c84b..c2b5e4afb5c 100644 --- a/sw/airborne/modules/time/flight_time.c +++ b/sw/airborne/modules/time/time_countdown.c @@ -21,26 +21,33 @@ */ /** - * @file modules/time/flight_time.c + * @file modules/time/time_countdown.c * - * Flight time counter that can be set from the gcs + * Count down remaining time. + * Set an initial countdown value in seconds (re-settable via settings/GCS) + * and countdown @a time_until_end variable to zero. + * E.g. allows to check how much time is left before the end of the competition. */ -#include "flight_time.h" +#include "time_countdown.h" #include "generated/airframe.h" -uint16_t time_until_land; +uint16_t time_until_end; -#ifndef FLIGHT_TIME_LEFT -#define FLIGHT_TIME_LEFT 10000 +/** initial countdown value in seconds */ +#ifndef TIME_COUNTDOWN_START_VALUE +#define TIME_COUNTDOWN_START_VALUE 10000 #endif -void flight_time_init(void) { - time_until_land = FLIGHT_TIME_LEFT; +void time_countdown_init(void) +{ + time_until_end = TIME_COUNTDOWN_START_VALUE; } -void flight_time_periodic( void ) { +void time_countdown_periodic_1hz(void) +{ // Count downwards - if(time_until_land > 0) - time_until_land--; + if (time_until_end > 0) { + time_until_end--; + } } diff --git a/sw/airborne/modules/time/flight_time.h b/sw/airborne/modules/time/time_countdown.h similarity index 65% rename from sw/airborne/modules/time/flight_time.h rename to sw/airborne/modules/time/time_countdown.h index 2a4ff41f894..a1d82f1a54f 100644 --- a/sw/airborne/modules/time/flight_time.h +++ b/sw/airborne/modules/time/time_countdown.h @@ -21,19 +21,22 @@ */ /** - * @file modules/time/flight_time.h + * @file modules/time/time_countdown.h * - * Flight time counter that can be set from the gcs + * Count down remaining time. + * Set an initial countdown value in seconds (re-settable via settings/GCS) + * and countdown @a time_until_end variable to zero. + * E.g. allows to check how much time is left before the end of the competition. */ -#ifndef FLIGHT_TIME_H -#define FLIGHT_TIME_H +#ifndef TIME_COUNTDOWN_H +#define TIME_COUNTDOWN_H #include "std.h" -extern uint16_t time_until_land; +extern uint16_t time_until_end; -void flight_time_init(void); -void flight_time_periodic(void); +void time_countdown_init(void); +void time_countdown_periodic_1hz(void); -#endif /* FLIGHT_TIME_H */ +#endif /* TIME_COUNTDOWN_H */ From fe02983febdb93855dd5dd6c1ecd4add2b6a1647 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 16:39:37 +0100 Subject: [PATCH 74/86] [messages] possibility to add descriptions --- conf/messages.dtd | 6 ++++-- conf/messages.xml | 12 +++++++----- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/conf/messages.dtd b/conf/messages.dtd index 4473bcee3c5..2e6072565fa 100644 --- a/conf/messages.dtd +++ b/conf/messages.dtd @@ -6,14 +6,16 @@ name CDATA #REQUIRED > - + - + + + + alive/heartbeat message containing the MD5sum of the aircraft configuration + Answer to PING datalink message, to measure latencies @@ -70,8 +72,8 @@ - - + Altitude above geoid (MSL) + norm of 2d ground speed in cm/s @@ -471,7 +473,7 @@ - + altitude above WGS84 reference ellipsoid @@ -1384,8 +1386,8 @@ - - + altitude above WGS84 reference ellipsoid + height above mean sea level (geoid) From c0074af11dda707dcad57d80ba8881f796e809a8 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 26 Nov 2014 17:57:37 +0100 Subject: [PATCH 75/86] [build] don't compile natnet if not needed --- sw/ground_segment/misc/Makefile | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index 0f72ee605ae..20dd6d6cbad 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -61,7 +61,7 @@ kestrel2ivy: kestrel2ivy.o @echo CC $@ $(Q)$(CC) $(CFLAGS) -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) $(IVY_LDFLAGS) -natnet2ivy: natnet2ivy.o $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.o $(PAPARAZZI_SRC)/sw/airborne/math/pprz_algebra_double.o $(PAPARAZZI_SRC)/sw/airborne/fms/fms_network.o +natnet2ivy: natnet2ivy.o pprz_geodetic_double.o pprz_algebra_double.o fms_network.o @echo CC $@ $(Q)$(CC) $(CFLAGS) -o natnet2ivy natnet2ivy.o pprz_geodetic_double.o pprz_algebra_double.o fms_network.o $(LIBRARYS) $(NATNET_LIBRARYS) $(IVY_LDFLAGS) @@ -69,6 +69,15 @@ video_synchronizer: video_synchronizer.c @echo CC $@ $(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) $(LIBRARYS) $(INCLUDES) -o $@ $< $(GTK_LDFLAGS) +pprz_algebra_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_algebra_double.c + $(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $< + +pprz_geodetic_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.c + $(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $< + +fms_network.o : $(PAPARAZZI_SRC)/sw/airborne/fms/fms_network.c + $(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $< + %.o : %.c $(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $< From c24b395589e56ac5ee45b7e6ce83ac21d6b4d425 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 15:43:27 +0100 Subject: [PATCH 76/86] [modules][messages] mission: LLA waypoints in 1e7deg instead of float A 32bit float has 23bits for the mantissa, which is a bit more than 7 digits. Since you already use 3 digits to represent the part to the left of the decimal point, that leaves about 4 digits for the rest.. So in the worst case of 180deg you have a resolution of only ~3m! With 1e7deg 32bit int you get ~1cm resolution. addresses first part of #984 (at least for the rotorcraft version, fixedwing version still converts to LLA float in between...) --- conf/messages.xml | 64 +++++++++---------- sw/airborne/modules/mission/mission_common.c | 44 ++++++------- sw/airborne/modules/mission/mission_common.h | 6 +- sw/airborne/modules/mission/mission_fw_nav.c | 10 ++- .../modules/mission/mission_rotorcraft_nav.c | 17 +++-- 5 files changed, 74 insertions(+), 67 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index cc61e70947c..f563e4f6906 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2319,16 +2319,16 @@ - + altitude above geoid (MSL) - - - + + + altitude above geoid (MSL) @@ -2337,7 +2337,7 @@ - + altitude above geoid (MSL) @@ -2345,9 +2345,9 @@ - - - + + + altitude above geoid (MSL) @@ -2359,18 +2359,18 @@ - + altitude above geoid (MSL)
- - - - - + + + + + altitude above geoid (MSL) @@ -2387,7 +2387,7 @@ - + altitude above geoid (MSL) @@ -2395,17 +2395,17 @@ - - - - - - - - - - - + + + + + + + + + + + altitude above geoid (MSL) @@ -2417,18 +2417,18 @@ - + altitude above geoid (MSL) - - - - - + + + + + altitude above geoid (MSL) diff --git a/sw/airborne/modules/mission/mission_common.c b/sw/airborne/modules/mission/mission_common.c index 09523dbd47d..eea9810da3d 100644 --- a/sw/airborne/modules/mission/mission_common.c +++ b/sw/airborne/modules/mission/mission_common.c @@ -137,9 +137,9 @@ int mission_parse_GOTO_WP(void) { int mission_parse_GOTO_WP_LLA(void) { if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft - struct LlaCoor_f lla; - lla.lat = RadOfDeg(DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer)); - lla.lon = RadOfDeg(DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer)); + struct LlaCoor_i lla; + lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer); + lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer); lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(dl_buffer); struct _mission_element me; @@ -172,9 +172,9 @@ int mission_parse_CIRCLE(void) { int mission_parse_CIRCLE_LLA(void) { if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft - struct LlaCoor_f lla; - lla.lat = RadOfDeg(DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer)); - lla.lon = RadOfDeg(DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer)); + struct LlaCoor_i lla; + lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer); + lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer); lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(dl_buffer); struct _mission_element me; @@ -210,12 +210,12 @@ int mission_parse_SEGMENT(void) { int mission_parse_SEGMENT_LLA(void) { if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft - struct LlaCoor_f from_lla, to_lla; - from_lla.lat = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer)); - from_lla.lon = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer)); + struct LlaCoor_i from_lla, to_lla; + from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer); + from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer); from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer); - to_lla.lat = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer)); - to_lla.lon = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer)); + to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer); + to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer); to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer); struct _mission_element me; @@ -263,21 +263,21 @@ int mission_parse_PATH(void) { int mission_parse_PATH_LLA(void) { if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft - struct LlaCoor_f lla[MISSION_PATH_NB]; - lla[0].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_1(dl_buffer)); - lla[0].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_1(dl_buffer)); + struct LlaCoor_i lla[MISSION_PATH_NB]; + lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(dl_buffer); + lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(dl_buffer); lla[0].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer); - lla[1].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_2(dl_buffer)); - lla[1].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_2(dl_buffer)); + lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(dl_buffer); + lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(dl_buffer); lla[1].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer); - lla[2].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_3(dl_buffer)); - lla[2].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_3(dl_buffer)); + lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(dl_buffer); + lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(dl_buffer); lla[2].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer); - lla[3].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_4(dl_buffer)); - lla[3].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_4(dl_buffer)); + lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(dl_buffer); + lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(dl_buffer); lla[3].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer); - lla[4].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_5(dl_buffer)); - lla[4].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_5(dl_buffer)); + lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(dl_buffer); + lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(dl_buffer); lla[4].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer); struct _mission_element me; diff --git a/sw/airborne/modules/mission/mission_common.h b/sw/airborne/modules/mission/mission_common.h index b76d5c89b10..4b1cec706d8 100644 --- a/sw/airborne/modules/mission/mission_common.h +++ b/sw/airborne/modules/mission/mission_common.h @@ -142,11 +142,11 @@ extern struct _mission_element * mission_get(void); /** Get the ENU component of LLA mission point * This function is firmware specific. - * @param point pointer to the output ENU point - * @param lla pointer to the input LLA coordinate + * @param point pointer to the output ENU point (float) + * @param lla pointer to the input LLA coordinates (int) * @return TRUE if conversion is succesful, FALSE otherwise */ -extern bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla); +extern bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla); /** Run mission * diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c index 0fdd296f733..d697beb615c 100644 --- a/sw/airborne/modules/mission/mission_fw_nav.c +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -34,12 +34,16 @@ #include "subsystems/navigation/common_nav.h" #include "generated/flight_plan.h" -// Utility function: converts lla to local point -bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) { +/// Utility function: converts lla (int) to local point (float) +bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { + /// TODO: don't convert to float, either use double or do completely in fixed point + struct LlaCoor_f lla_f; + LLA_FLOAT_OF_BFP(lla_f, *lla); + /* Computes from (lat, long) in the referenced UTM zone */ struct UtmCoor_f utm; utm.zone = nav_utm_zone0; - utm_of_lla_f(&utm, lla); + utm_of_lla_f(&utm, &lla_f); /* Computes relative position to HOME waypoint * and bound the distance to max_dist_from_home diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c index 3a324efbca9..76c55911836 100644 --- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c +++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c @@ -37,23 +37,26 @@ //Buffer zone in [m] before MAX_DIST_FROM_HOME #define BUFFER_ZONE_DIST 10 -// Utility function: converts lla to local point -bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) { +/// Utility function: converts lla (int) to local point (float) +bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { // return FALSE if there is no valid local coordinate // FIXME we should only test if local frame is initialized, not valid if (!stateIsLocalCoordinateValid()) return FALSE; // change geoid alt to ellipsoid alt - lla->alt = lla->alt - state.ned_origin_f.hmsl + state.ned_origin_f.lla.alt; + lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; + //Compute ENU components from LLA with respect to ltp origin - struct EnuCoor_f tmp_enu_point; - enu_of_lla_point_f(&tmp_enu_point, &state.ned_origin_f, lla); + struct EnuCoor_i tmp_enu_point_i; + enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla); + struct EnuCoor_f tmp_enu_point_f; + ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i); //Bound the new waypoint with max distance from home struct EnuCoor_f home; ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]); struct FloatVect2 vect_from_home; - VECT2_DIFF(vect_from_home, tmp_enu_point, home); + VECT2_DIFF(vect_from_home, tmp_enu_point_f, home); //Saturate the mission wp not to overflow max_dist_from_home //including a buffer zone before limits float dist_to_home = float_vect2_norm(&vect_from_home); @@ -63,7 +66,7 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) { } // set new point VECT2_SUM(*point, home, vect_from_home); - point->z = tmp_enu_point.z; + point->z = tmp_enu_point_f.z; return TRUE; } From f15cd031e8788113c34119bf5dd7f21dfe47b04f Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 16:12:18 +0100 Subject: [PATCH 77/86] [modules] mission_rotorcraft: only check for ned_initialized_i --- sw/airborne/modules/mission/mission_rotorcraft_nav.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c index 76c55911836..44fadf73e44 100644 --- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c +++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c @@ -39,9 +39,9 @@ /// Utility function: converts lla (int) to local point (float) bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { - // return FALSE if there is no valid local coordinate - // FIXME we should only test if local frame is initialized, not valid - if (!stateIsLocalCoordinateValid()) return FALSE; + // return FALSE if there is no valid local coordinate system + if (!state.ned_initialized_i) + return FALSE; // change geoid alt to ellipsoid alt lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; From 59bcb4eca1fabac69e87f8380fb64eb8477ec1de Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 26 Nov 2014 18:57:54 +0100 Subject: [PATCH 78/86] [messages] remove description node again for now seems server/GCS (OCaml lib) can't handle it, as they fail with Fatal error: exception ExtXml.Error("Error: Attribute 'type' expected in <<>>") --- conf/messages.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index f563e4f6906..292fcfefb44 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -9,12 +9,10 @@ - alive/heartbeat message containing the MD5sum of the aircraft configuration - Answer to PING datalink message, to measure latencies From 4c6d5a8e5a5da895ab3ebbc42896119867ac5d2a Mon Sep 17 00:00:00 2001 From: podhrmic Date: Wed, 26 Nov 2014 15:01:05 -0800 Subject: [PATCH 79/86] [submodules] updated to a new chibios repository --- .gitmodules | 6 +++--- sw/ext/chibios | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index 08e8ed60b61..1522a585fca 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,12 +4,12 @@ [submodule "sw/ext/luftboot"] path = sw/ext/luftboot url = https://github.com/paparazzi/luftboot.git -[submodule "sw/ext/chibios"] - path = sw/ext/chibios - url = https://github.com/ChibiOS/ChibiOS-RT.git [submodule "sw/ext/fatfs"] path = sw/ext/fatfs url = https://github.com/enacuavlab/fatfs.git [submodule "sw/ext/ardrone2_vision"] path = sw/ext/ardrone2_vision url = https://github.com/tudelft/ardrone2_vision.git +[submodule "sw/ext/chibios"] + path = sw/ext/chibios + url = https://github.com/ChibiOS/ChibiOS-gitmain.git diff --git a/sw/ext/chibios b/sw/ext/chibios index 1804611c437..f73847b4521 160000 --- a/sw/ext/chibios +++ b/sw/ext/chibios @@ -1 +1 @@ -Subproject commit 1804611c43765b6d7328448d7b60cf165de6ed16 +Subproject commit f73847b45215002aba42d1a56a117114c9e5ba01 From 855ac854c6b89939e19908570b0b8d1669738d9b Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Nov 2014 13:10:38 +0100 Subject: [PATCH 80/86] [ocaml] filter for 'field' tag when looking at messages --- sw/ground_segment/tmtc/messages.ml | 3 ++- sw/lib/ocaml/pprz.ml | 4 +++- sw/simulator/sitl.ml | 6 +----- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index e9cedcda209..377984599b1 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -48,6 +48,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m -> let _l = GMisc.label ~text:id ~packing:h#add () in let eb = GBin.event_box ~packing:h#pack () in let time = GMisc.label ~width:40 ~packing:eb#add () in + let fields = List.filter (fun f -> Xml.tag f = "field") (Xml.children m) in eb#coerce#misc#modify_bg [`SELECTED, `NAME "green"]; let fields = List.fold_left @@ -105,7 +106,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) bind m -> rest ) [] - (Xml.children m) + fields in let (update_values, display_values) = List.split fields in let n = List.length fields in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index 85542939d7c..a2146129f8c 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -353,9 +353,11 @@ let parse_class = fun xml_class -> with Xml.No_attribute("link") -> None in + (* only keep a "field" nodes *) + let xml_children = List.filter (fun f -> Xml.tag f = "field") (Xml.children xml_msg) in let msg = { name = name; - fields = List.map field_of_xml (Xml.children xml_msg); + fields = List.map field_of_xml xml_children; link = link } in let id = int_of_string (ExtXml.attrib xml_msg "id") in diff --git a/sw/simulator/sitl.ml b/sw/simulator/sitl.ml index 25812139998..b7cb05924e9 100644 --- a/sw/simulator/sitl.ml +++ b/sw/simulator/sitl.ml @@ -46,11 +46,7 @@ module Make (A:Data.MISSION) (FM: FlightModel.SIG) = struct let rc_period = 1./.40. (* s *) let sys_time_period = 1./.120. (* s *) - let msg = fun name -> - ExtXml.child Data.messages_ap ~select:(fun x -> ExtXml.attrib x "name" = name) "message" - - -(* Commands handling (rcommands is the intermediate storage) *) + (* Commands handling (rcommands is the intermediate storage) *) let rc_channels = Array.of_list (Xml.children A.ac.Data.radio) let nb_channels = Array.length rc_channels let rc_channel_no = fun x -> From 2cdc1334bd9ffbd2451247dde627331d0e096050 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Nov 2014 13:37:08 +0100 Subject: [PATCH 81/86] [chibios] switch back to correct version --- sw/ext/chibios | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/ext/chibios b/sw/ext/chibios index f73847b4521..1804611c437 160000 --- a/sw/ext/chibios +++ b/sw/ext/chibios @@ -1 +1 @@ -Subproject commit f73847b45215002aba42d1a56a117114c9e5ba01 +Subproject commit 1804611c43765b6d7328448d7b60cf165de6ed16 From 6be781d36b8333f7eeb5d7d237a83f32f8be4be7 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Nov 2014 14:00:46 +0100 Subject: [PATCH 82/86] [chibios] switch back to a repo that have to 2.6 stable --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index 1522a585fca..c4c64f44459 100644 --- a/.gitmodules +++ b/.gitmodules @@ -12,4 +12,4 @@ url = https://github.com/tudelft/ardrone2_vision.git [submodule "sw/ext/chibios"] path = sw/ext/chibios - url = https://github.com/ChibiOS/ChibiOS-gitmain.git + url = https://github.com/mabl/ChibiOS-gitmain.git From 223f108ab524d72d84641a9bf0ac926a6c501dd9 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 27 Nov 2014 14:05:42 +0100 Subject: [PATCH 83/86] fix name of git submodule --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index c4c64f44459..bf9e3ff49bc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -12,4 +12,4 @@ url = https://github.com/tudelft/ardrone2_vision.git [submodule "sw/ext/chibios"] path = sw/ext/chibios - url = https://github.com/mabl/ChibiOS-gitmain.git + url = https://github.com/mabl/ChibiOS.git From 1120d8d02a1161d92fe486a3a68141d5e60a8b7d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 27 Nov 2014 14:35:44 +0100 Subject: [PATCH 84/86] [messages] add some descriptions again --- conf/messages.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/conf/messages.xml b/conf/messages.xml index 292fcfefb44..f563e4f6906 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -9,10 +9,12 @@ + alive/heartbeat message containing the MD5sum of the aircraft configuration + Answer to PING datalink message, to measure latencies From 321c583c5dbf7909f3e42cd23a9312b5abc4ee49 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 27 Nov 2014 14:35:18 +0100 Subject: [PATCH 85/86] [python] fix settings path --- sw/lib/python/settings_xml_parse.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/lib/python/settings_xml_parse.py b/sw/lib/python/settings_xml_parse.py index 98a27e28404..5f2602221c5 100755 --- a/sw/lib/python/settings_xml_parse.py +++ b/sw/lib/python/settings_xml_parse.py @@ -31,8 +31,8 @@ def __init__(self, ac_id): # save AC name for reference self.name = ac_node[0].attrib['name'] - # get settings.xml file path from var/ directory - settings_xml_path = os.path.join(paparazzi_home, 'var/' + self.name + '/settings.xml') + # get settings.xml file path from var/aircrafts/ directory + settings_xml_path = os.path.join(paparazzi_home, 'var/aircrafts/' + self.name + '/settings.xml') if not os.path.isfile(settings_xml_path): print("Could not find %s, did you build it?" % settings_xml_path) From d0042097065cc4a10b2c86be7e6eca0b8925246e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 27 Nov 2014 17:09:13 +0100 Subject: [PATCH 86/86] [ahrs] update/clean test_ahrs --- conf/airframes/examples/quadrotor_lisa_mx.xml | 7 + .../fixedwing/ahrs_float_cmpl.makefile | 1 - .../fixedwing/ahrs_float_cmpl_quat.makefile | 2 +- .../fixedwing/ahrs_float_cmpl_rmat.makefile | 10 +- .../subsystems/fixedwing/ahrs_ic.makefile | 2 - .../subsystems/rotorcraft/ahrs_cmpl.makefile | 1 - .../rotorcraft/ahrs_float_cmpl.makefile | 1 - .../subsystems/rotorcraft/ahrs_ic.makefile | 1 - .../ahrs_float_cmpl_quat.makefile | 3 + .../ahrs_float_cmpl_rmat.makefile | 3 + .../ahrs_float_mlkf.makefile | 3 + .../shared/ahrs_int_cmpl_euler.makefile | 3 + .../ahrs_int_cmpl_quat.makefile | 3 + .../shared/imu_lisa_m_or_mx_v2.1.makefile | 3 + conf/firmwares/test_progs.makefile | 23 ++- sw/airborne/test/subsystems/test_ahrs.c | 156 +++++------------- 16 files changed, 97 insertions(+), 125 deletions(-) delete mode 100644 conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl.makefile delete mode 100644 conf/firmwares/subsystems/fixedwing/ahrs_ic.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/ahrs_cmpl.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/ahrs_ic.makefile rename conf/firmwares/subsystems/{rotorcraft => shared}/ahrs_float_cmpl_quat.makefile (93%) rename conf/firmwares/subsystems/{rotorcraft => shared}/ahrs_float_cmpl_rmat.makefile (93%) rename conf/firmwares/subsystems/{rotorcraft => shared}/ahrs_float_mlkf.makefile (93%) rename conf/firmwares/subsystems/{rotorcraft => shared}/ahrs_int_cmpl_quat.makefile (93%) diff --git a/conf/airframes/examples/quadrotor_lisa_mx.xml b/conf/airframes/examples/quadrotor_lisa_mx.xml index a65f31df4eb..4f4ed1a5c86 100644 --- a/conf/airframes/examples/quadrotor_lisa_mx.xml +++ b/conf/airframes/examples/quadrotor_lisa_mx.xml @@ -52,6 +52,13 @@ + + + + + + + diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl.makefile deleted file mode 100644 index 839cd8003f6..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error The ahrs_float_cmpl subsystem has been split, please replace with in your airframe file.) diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile index ceec6d9d86f..efa3ababb41 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_quat.makefile @@ -8,7 +8,7 @@ USE_MAGNETOMETER ?= 0 AHRS_ALIGNER_LED ?= none -AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile index 51d63fb73a8..1202eac9655 100644 --- a/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/fixedwing/ahrs_float_cmpl_rmat.makefile @@ -8,7 +8,7 @@ USE_MAGNETOMETER ?= 0 AHRS_ALIGNER_LED ?= none -AHRS_CFLAGS = -DUSE_AHRS +AHRS_CFLAGS = -DUSE_AHRS -DAHRS_FLOAT AHRS_CFLAGS += -DUSE_AHRS_ALIGNER -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN ifeq (,$(findstring $(USE_MAGNETOMETER),0 FALSE)) @@ -27,6 +27,14 @@ AHRS_SRCS += subsystems/ahrs.c AHRS_SRCS += subsystems/ahrs/ahrs_float_cmpl.c AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c +ifdef AHRS_PROPAGATE_FREQUENCY +AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY) +endif + +ifdef AHRS_CORRECT_FREQUENCY +AHRS_CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY) +endif + ap.CFLAGS += $(AHRS_CFLAGS) ap.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/fixedwing/ahrs_ic.makefile b/conf/firmwares/subsystems/fixedwing/ahrs_ic.makefile deleted file mode 100644 index 29b7942374b..00000000000 --- a/conf/firmwares/subsystems/fixedwing/ahrs_ic.makefile +++ /dev/null @@ -1,2 +0,0 @@ -$(error The ahrs_ic subsystem has been renamed, please replace with in your airframe file.) - diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_cmpl.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_cmpl.makefile deleted file mode 100644 index 0260d9d5e2e..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_cmpl.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error The ahrs_cmpl subsystem has been renamed, please replace with in your airframe file.) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl.makefile deleted file mode 100644 index 839cd8003f6..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error The ahrs_float_cmpl subsystem has been split, please replace with in your airframe file.) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_ic.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_ic.makefile deleted file mode 100644 index f91ed066fc2..00000000000 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_ic.makefile +++ /dev/null @@ -1 +0,0 @@ -$(error The ahrs_ic subsystem has been renamed, please replace with in your airframe file.) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile similarity index 93% rename from conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile rename to conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile index 78ba4664ba0..9f45359b399 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_quat.makefile @@ -38,3 +38,6 @@ ap.srcs += $(AHRS_SRCS) nps.CFLAGS += $(AHRS_CFLAGS) nps.srcs += $(AHRS_SRCS) + +test_ahrs.CFLAGS += $(AHRS_CFLAGS) +test_ahrs.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile similarity index 93% rename from conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile rename to conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile index 9aed5710826..b195db2edd1 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_cmpl_rmat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_cmpl_rmat.makefile @@ -37,3 +37,6 @@ ap.srcs += $(AHRS_SRCS) nps.CFLAGS += $(AHRS_CFLAGS) nps.srcs += $(AHRS_SRCS) + +test_ahrs.CFLAGS += $(AHRS_CFLAGS) +test_ahrs.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile similarity index 93% rename from conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile rename to conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile index 8f9d1d5d9c0..51932344375 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_float_mlkf.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_float_mlkf.makefile @@ -36,3 +36,6 @@ ap.srcs += $(AHRS_SRCS) nps.CFLAGS += $(AHRS_CFLAGS) nps.srcs += $(AHRS_SRCS) + +test_ahrs.CFLAGS += $(AHRS_CFLAGS) +test_ahrs.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile index f51c9c4d476..80f9d1e0624 100644 --- a/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_euler.makefile @@ -42,3 +42,6 @@ sim.srcs += $(ahrssim_srcs) jsbsim.CFLAGS += $(ahrssim_CFLAGS) jsbsim.srcs += $(ahrssim_srcs) + +test_ahrs.CFLAGS += $(AHRS_CFLAGS) +test_ahrs.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile similarity index 93% rename from conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile rename to conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile index 310df57d6b4..44357ace4d1 100644 --- a/conf/firmwares/subsystems/rotorcraft/ahrs_int_cmpl_quat.makefile +++ b/conf/firmwares/subsystems/shared/ahrs_int_cmpl_quat.makefile @@ -37,3 +37,6 @@ ap.srcs += $(AHRS_SRCS) nps.CFLAGS += $(AHRS_CFLAGS) nps.srcs += $(AHRS_SRCS) + +test_ahrs.CFLAGS += $(AHRS_CFLAGS) +test_ahrs.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile b/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile index 48875155878..b9696ba115d 100644 --- a/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile +++ b/conf/firmwares/subsystems/shared/imu_lisa_m_or_mx_v2.1.makefile @@ -69,6 +69,9 @@ ap.srcs += $(IMU_ASPIRIN_2_SRCS) test_imu.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) test_imu.srcs += $(IMU_ASPIRIN_2_SRCS) +test_ahrs.CFLAGS += $(IMU_ASPIRIN_2_CFLAGS) +test_ahrs.srcs += $(IMU_ASPIRIN_2_SRCS) + # # NPS simulator # diff --git a/conf/firmwares/test_progs.makefile b/conf/firmwares/test_progs.makefile index 1243d97df03..c062c89644d 100644 --- a/conf/firmwares/test_progs.makefile +++ b/conf/firmwares/test_progs.makefile @@ -333,7 +333,28 @@ test_imu.CFLAGS += $(COMMON_TELEMETRY_CFLAGS) test_imu.srcs += $(COMMON_TELEMETRY_SRCS) test_imu.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c test_imu.srcs += test/subsystems/test_imu.c -test_imu.srcs += math/pprz_trig_int.c +test_imu.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c + + +# +# test_ahrs +# +# add imu and ahrs subsystems to test_ahrs target! +# +# configuration +# SYS_TIME_LED +# MODEM_PORT +# MODEM_BAUD +# +test_ahrs.ARCHDIR = $(ARCH) +test_ahrs.CFLAGS += $(COMMON_TEST_CFLAGS) +test_ahrs.srcs += $(COMMON_TEST_SRCS) +test_ahrs.CFLAGS += $(COMMON_TELEMETRY_CFLAGS) -DPERIODIC_TELEMETRY +test_ahrs.srcs += $(COMMON_TELEMETRY_SRCS) +test_ahrs.srcs += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_arch.c +test_ahrs.srcs += test/subsystems/test_ahrs.c +test_ahrs.srcs += state.c +test_ahrs.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c math/pprz_trig_int.c math/pprz_orientation_conversion.c math/pprz_algebra_int.c math/pprz_algebra_float.c math/pprz_algebra_double.c # diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index f692840b0f4..f9bbf582e60 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -21,6 +21,13 @@ #include +/* PERIODIC_C_MAIN is defined before generated/periodic_telemetry.h + * in order to implement telemetry_mode_Main_* + */ +#define PERIODIC_C_MAIN + +#include "generated/periodic_telemetry.h" + #include "std.h" #include "mcu.h" #include "mcu_periph/sys_time.h" @@ -29,60 +36,61 @@ #include "mcu_periph/i2c.h" #include "messages.h" #include "subsystems/datalink/downlink.h" +#include "subsystems/datalink/telemetry.h" #include "subsystems/imu.h" #include "subsystems/ahrs.h" #include "subsystems/ahrs/ahrs_aligner.h" -#include "my_debug_servo.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static inline void main_report(void); static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_mag_event(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { - +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); imu_init(); ahrs_aligner_init(); ahrs_init(); - DEBUG_SERVO1_INIT(); - // DEBUG_SERVO2_INIT(); - mcu_int_enable(); } -static inline void main_periodic_task( void ) { - - if (sys_time.nb_sec > 1) imu_periodic(); +static inline void main_periodic_task(void) +{ + if (sys_time.nb_sec > 1) { + imu_periodic(); + } RunOnceEvery(10, { LED_PERIODIC();}); main_report(); } -static inline void main_event_task( void ) { - +static inline void main_event_task(void) +{ ImuEvent(on_gyro_event, on_accel_event, on_mag_event); - } -static inline void on_gyro_event(void) { +static inline void on_gyro_event(void) +{ // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -94,17 +102,16 @@ static inline void on_gyro_event(void) { imu_scale_gyro(&imu); if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); - } - else { - DEBUG_S1_ON(); + } + } else { ahrs_propagate(dt); - DEBUG_S1_OFF(); } } -static inline void on_accel_event(void) { +static inline void on_accel_event(void) +{ // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -115,13 +122,12 @@ static inline void on_accel_event(void) { imu_scale_accel(&imu); if (ahrs.status != AHRS_UNINIT) { - DEBUG_S2_ON(); ahrs_update_accel(dt); - DEBUG_S2_OFF(); } } -static inline void on_mag_event(void) { +static inline void on_mag_event(void) +{ // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -137,91 +143,9 @@ static inline void on_mag_event(void) { } -static inline void main_report(void) { - - PeriodicPrescaleBy10( - { - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - }, - { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - }, - { - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); - }, - { - DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); - }, - { - DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); - }, - - { - DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - }, - - { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - }, - { -#if USE_I2C2 - uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; - uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; - uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; - uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; - uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; - uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; - uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; - uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; - uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; - uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; - const uint8_t _bus2 = 2; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, - &i2c2_queue_full_cnt, - &i2c2_ack_fail_cnt, - &i2c2_miss_start_stop_cnt, - &i2c2_arb_lost_cnt, - &i2c2_over_under_cnt, - &i2c2_pec_recep_cnt, - &i2c2_timeout_tlow_cnt, - &i2c2_smbus_alert_cnt, - &i2c2_unexpected_event_cnt, - &i2c2_last_unexpected_event, - &_bus2); -#endif - }, - { - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - &ahrs.ltp_to_imu_euler.phi, - &ahrs.ltp_to_imu_euler.theta, - &ahrs.ltp_to_imu_euler.psi, - &ahrs.ltp_to_body_euler.phi, - &ahrs.ltp_to_body_euler.theta, - &ahrs.ltp_to_body_euler.psi); - }, - { - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, - &ahrs_impl.gyro_bias.p, - &ahrs_impl.gyro_bias.q, - &ahrs_impl.gyro_bias.r); - - }); +static inline void main_report(void) +{ + RunOnceEvery(512, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM)); + + periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); }