From 9da16afcc2985503523460c4629343a556ef40d7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 08:59:48 +0200 Subject: [PATCH 01/12] Add support for V for quads with offset arms such as the TBS and SteadiDrone QU4D --- apps/systemlib/mixer/mixer.h | 1 + apps/systemlib/mixer/mixer_multirotor.cpp | 11 +++++++++++ apps/systemlib/mixer/multi_tables | 9 ++++++++- 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 71386cba77..40d37fce2b 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -418,6 +418,7 @@ public: enum Geometry { QUAD_X = 0, /**< quad in X configuration */ QUAD_PLUS, /**< quad in + configuration */ + QUAD_V, /**< quad in V configuration */ HEX_X, /**< hex in X configuration */ HEX_PLUS, /**< hex in + configuration */ OCTA_X, diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index 4b9cfc023a..a45ca3f21a 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -82,6 +82,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { 0.000000, 1.000000, -1.00 }, { -0.000000, -1.000000, -1.00 }, }; +const MultirotorMixer::Rotor _config_quad_v[] = { + { -0.882948, 0.469472, 1.00 }, + { 0.731354, -0.681998, 1.00 }, + { 0.882948, 0.469472, -1.00 }, + { -0.731354, -0.681998, -1.00 }, +}; const MultirotorMixer::Rotor _config_hex_x[] = { { -1.000000, 0.000000, -1.00 }, { 1.000000, 0.000000, 1.00 }, @@ -121,6 +127,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], + &_config_quad_v[0], &_config_hex_x[0], &_config_hex_plus[0], &_config_octa_x[0], @@ -129,6 +136,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ + 4, /* quad_v */ 6, /* hex_x */ 6, /* hex_plus */ 8, /* octa_x */ @@ -184,6 +192,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4x")) { geometry = MultirotorMixer::QUAD_X; + } else if (!strcmp(geomname, "4v")) { + geometry = MultirotorMixer::QUAD_V; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorMixer::HEX_PLUS; diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables index f17ae30ca6..0c5689143b 100755 --- a/apps/systemlib/mixer/multi_tables +++ b/apps/systemlib/mixer/multi_tables @@ -20,6 +20,13 @@ set quad_plus { 180 CW } +set quad_v { + 62 CCW + -133 CCW + -62 CW + 133 CW +} + set hex_x { 90 CW -90 CCW @@ -60,7 +67,7 @@ set octa_plus { 90 CW } -set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus} +set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} From 1063ae9de9f2efe7b8c468936695255f252a08f4 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 21:40:43 +0200 Subject: [PATCH 02/12] Add a mixer file for the V quad --- ROMFS/mixers/FMU_quad_v.mix | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 ROMFS/mixers/FMU_quad_v.mix diff --git a/ROMFS/mixers/FMU_quad_v.mix b/ROMFS/mixers/FMU_quad_v.mix new file mode 100644 index 0000000000..2a4a0f3419 --- /dev/null +++ b/ROMFS/mixers/FMU_quad_v.mix @@ -0,0 +1,7 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in the V configuration. All controls +are mixed 100%. + +R: 4v 10000 10000 10000 0 From a6b8463308d62be1657fb773445375b035a6765f Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 25 Apr 2013 23:44:22 +0200 Subject: [PATCH 03/12] Update the makefile to include the new mixer config. --- ROMFS/Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/Makefile b/ROMFS/Makefile index ed39ab8257..11a4650fa6 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -32,6 +32,7 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \ $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \ + $(SRCROOT)/mixers/FMU_quad_v.mix~mixers/FMU_quad_v.mix \ $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \ $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \ $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \ From afbb4d55b3fc4e0e2a1fc1a1b052e9f4fb034d51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 15:13:47 +0200 Subject: [PATCH 04/12] Finished conversion to C++ --- apps/attitude_estimator_ekf/Makefile | 5 +++-- ...f_main.c => attitude_estimator_ekf_main.cpp} | 17 ++++++++++++----- 2 files changed, 15 insertions(+), 7 deletions(-) rename apps/attitude_estimator_ekf/{attitude_estimator_ekf_main.c => attitude_estimator_ekf_main.cpp} (98%) diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 734af7cc1c..46a54c6607 100755 --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -35,8 +35,9 @@ APPNAME = attitude_estimator_ekf PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 -CSRCS = attitude_estimator_ekf_main.c \ - attitude_estimator_ekf_params.c \ +CXXSRCS = attitude_estimator_ekf_main.cpp + +CSRCS = attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp similarity index 98% rename from apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c rename to apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index bd972f03f3..1a50dde0f4 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -66,11 +66,17 @@ #include #include +#ifdef __cplusplus +extern "C" { +#endif #include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter.h" #include "attitude_estimator_ekf_params.h" +#ifdef __cplusplus +} +#endif -__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); +extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -265,10 +271,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* Main loop*/ while (!thread_should_exit) { - struct pollfd fds[2] = { - { .fd = sub_raw, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN } - }; + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; int ret = poll(fds, 2, 1000); if (ret < 0) { From 556a017444b809c18e2ce495a2fd00380960e0f4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Apr 2013 17:41:46 +0200 Subject: [PATCH 05/12] Hotfix: GPS MAVLink transmission fixes --- apps/mavlink/orb_listener.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 1dd3fc2d8e..5f15facf87 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l) /* copy gps data into local buffer */ orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + /* GPS COG is 0..2PI in degrees * 1e2 */ + float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) + cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; + + /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps.timestamp_position, @@ -236,13 +243,14 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - (uint16_t)(gps.eph_m * 1e2f), // from m to cm - (uint16_t)(gps.epv_m * 1e2f), // from m to cm - (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s - (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 + gps.eph_m * 1e2f, // from m to cm + gps.epv_m * 1e2f, // from m to cm + gps.vel_m_s * 1e2f, // from m/s to cm/s + cog_deg * 1e2f, // from rad to deg * 100 gps.satellites_visible); - if (gps.satellite_info_available && (gps_counter % 4 == 0)) { + /* update SAT info every 10 seconds */ + if (gps.satellite_info_available && (gps_counter % 50 == 0)) { mavlink_msg_gps_status_send(MAVLINK_COMM_0, gps.satellites_visible, gps.satellite_prn, From d6e9a35aa2eeda921ad8de93578db2ee69060ef5 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 26 Apr 2013 18:34:12 +0200 Subject: [PATCH 06/12] Update the quad V values. --- apps/systemlib/mixer/mixer_multirotor.cpp | 8 ++++---- apps/systemlib/mixer/multi_tables | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp index a45ca3f21a..d79811c0ff 100644 --- a/apps/systemlib/mixer/mixer_multirotor.cpp +++ b/apps/systemlib/mixer/mixer_multirotor.cpp @@ -83,10 +83,10 @@ const MultirotorMixer::Rotor _config_quad_plus[] = { { -0.000000, -1.000000, -1.00 }, }; const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.882948, 0.469472, 1.00 }, - { 0.731354, -0.681998, 1.00 }, - { 0.882948, 0.469472, -1.00 }, - { -0.731354, -0.681998, -1.00 }, + { -0.927184, 0.374607, 1.00 }, + { 0.694658, -0.719340, 1.00 }, + { 0.927184, 0.374607, -1.00 }, + { -0.694658, -0.719340, -1.00 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { { -1.000000, 0.000000, -1.00 }, diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables index 0c5689143b..19a8239a6d 100755 --- a/apps/systemlib/mixer/multi_tables +++ b/apps/systemlib/mixer/multi_tables @@ -21,10 +21,10 @@ set quad_plus { } set quad_v { - 62 CCW - -133 CCW - -62 CW - 133 CW + 68 CCW + -136 CCW + -68 CW + 136 CW } set hex_x { From ff15efb9c91540303622c3f9c48e22bbc9488ae0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 12:59:12 -0700 Subject: [PATCH 07/12] Build utility apps -Os to save ROM space. --- apps/examples/nsh/Makefile | 2 ++ apps/nshlib/Makefile | 2 ++ apps/px4/tests/Makefile | 2 ++ apps/system/i2c/Makefile | 1 + apps/systemcmds/bl_update/Makefile | 2 ++ apps/systemcmds/boardinfo/Makefile | 2 ++ apps/systemcmds/calibration/Makefile | 2 ++ apps/systemcmds/delay_test/Makefile | 2 ++ apps/systemcmds/eeprom/Makefile | 2 ++ apps/systemcmds/mixer/Makefile | 2 ++ apps/systemcmds/param/Makefile | 2 ++ apps/systemcmds/perf/Makefile | 2 ++ apps/systemcmds/preflight_check/Makefile | 2 ++ apps/systemcmds/reboot/Makefile | 2 ++ apps/systemcmds/top/Makefile | 2 ++ 15 files changed, 29 insertions(+) diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile index c7d212fc2c..ad687958f6 100644 --- a/apps/examples/nsh/Makefile +++ b/apps/examples/nsh/Makefile @@ -64,6 +64,8 @@ ROOTDEPPATH = --dep-path . VPATH = +MAXOPTIMIZATION = -Os + all: .built .PHONY: clean depend distclean diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile index 76cdac40d5..4256a10918 100644 --- a/apps/nshlib/Makefile +++ b/apps/nshlib/Makefile @@ -107,6 +107,8 @@ endif ROOTDEPPATH = --dep-path . VPATH = +MAXOPTIMIZATION = -Os + # Build targets all: .built diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile index cb1c3c6185..34f058be44 100644 --- a/apps/px4/tests/Makefile +++ b/apps/px4/tests/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 12000 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile index 1ed7a2faef..c98e2c0e2b 100644 --- a/apps/system/i2c/Makefile +++ b/apps/system/i2c/Makefile @@ -64,6 +64,7 @@ VPATH = APPNAME = i2c PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 +MAXOPTIMIZATION = -Os # Build targets diff --git a/apps/systemcmds/bl_update/Makefile b/apps/systemcmds/bl_update/Makefile index 9d0e156f60..d05493577a 100644 --- a/apps/systemcmds/bl_update/Makefile +++ b/apps/systemcmds/bl_update/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile index 753a6843e5..6f1be149c6 100644 --- a/apps/systemcmds/boardinfo/Makefile +++ b/apps/systemcmds/boardinfo/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile index aa1aa77610..a1735962e1 100644 --- a/apps/systemcmds/calibration/Makefile +++ b/apps/systemcmds/calibration/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_MAX - 1 STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/delay_test/Makefile b/apps/systemcmds/delay_test/Makefile index d30fcba27b..e54cf2f4e4 100644 --- a/apps/systemcmds/delay_test/Makefile +++ b/apps/systemcmds/delay_test/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile index 2f3db0fdce..79a05550ec 100644 --- a/apps/systemcmds/eeprom/Makefile +++ b/apps/systemcmds/eeprom/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile index b016ddc578..3d8ac38cb7 100644 --- a/apps/systemcmds/mixer/Makefile +++ b/apps/systemcmds/mixer/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/param/Makefile b/apps/systemcmds/param/Makefile index 603746a206..f19cadbb60 100644 --- a/apps/systemcmds/param/Makefile +++ b/apps/systemcmds/param/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile index 0134c9948b..f8bab41b65 100644 --- a/apps/systemcmds/perf/Makefile +++ b/apps/systemcmds/perf/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile index f138e2640b..98aadaa86f 100644 --- a/apps/systemcmds/preflight_check/Makefile +++ b/apps/systemcmds/preflight_check/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile index 9609a24fd0..15dd199829 100644 --- a/apps/systemcmds/reboot/Makefile +++ b/apps/systemcmds/reboot/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 2048 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile index c45775f4be..f58f9212e7 100644 --- a/apps/systemcmds/top/Makefile +++ b/apps/systemcmds/top/Makefile @@ -40,3 +40,5 @@ PRIORITY = SCHED_PRIORITY_DEFAULT - 10 STACKSIZE = 3000 include $(APPDIR)/mk/app.mk + +MAXOPTIMIZATION = -Os From de412b6467378ea5d4e61928f5795f309012a081 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 12:59:35 -0700 Subject: [PATCH 08/12] Pass -g to the link phase for PX4IO the same way we do for FMU --- nuttx/configs/px4io/common/Make.defs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nuttx/configs/px4io/common/Make.defs b/nuttx/configs/px4io/common/Make.defs index 4958f70926..7f782b5b22 100644 --- a/nuttx/configs/px4io/common/Make.defs +++ b/nuttx/configs/px4io/common/Make.defs @@ -112,7 +112,6 @@ ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") ARCHOPTIMIZATION += -g -ARCHSCRIPT += -g endif ARCHCFLAGS = -std=gnu99 @@ -145,7 +144,7 @@ ARCHDEFINES = ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 # this seems to be the only way to add linker flags -ARCHSCRIPT += --warn-common \ +EXTRA_LIBS += --warn-common \ --gc-sections CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common From 74c62a131e34d416ef29436b12d78f961e14807a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:12 -0700 Subject: [PATCH 09/12] Fix the way that we idle the tone_alarm pin so that the board defines what is the 'safe' state. --- apps/drivers/stm32/tone_alarm/tone_alarm.cpp | 8 ++++---- nuttx/configs/px4fmu/include/board.h | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index baa652d8ad..ac5511e60a 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -494,7 +494,7 @@ ToneAlarm::init() return ret; /* configure the GPIO to the idle state */ - stm32_configgpio(GPIO_TONE_ALARM); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); /* clock/power on our timer */ modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE); @@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note) rEGR = GTIM_EGR_UG; // force a reload of the period rCCER |= TONE_CCER; // enable the output + // configure the GPIO to enable timer output + stm32_configgpio(GPIO_TONE_ALARM); } void @@ -616,10 +618,8 @@ ToneAlarm::stop_note() /* * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + stm32_configgpio(GPIO_TONE_ALARM_IDLE); } void diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 8ad56a4c6a..0bc0b30216 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -326,6 +326,7 @@ */ #define TONE_ALARM_TIMER 3 /* timer 3 */ #define TONE_ALARM_CHANNEL 3 /* channel 3 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8) /************************************************************************************ From 264cf65d0f6b1259c64aad7d7bb45aff155a8e35 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:27 -0700 Subject: [PATCH 10/12] Fix an error in a #error --- apps/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index bb67d5e6d2..cec0c49ffd 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -125,7 +125,7 @@ # define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC # define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN # if CONFIG_STM32_TIM8 -# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 +# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8 # endif #elif HRT_TIMER == 9 # define HRT_TIMER_BASE STM32_TIM9_BASE From 46085b43d14b1ea1237aa176460ff648e0ff2d2a Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 26 Apr 2013 13:00:48 -0700 Subject: [PATCH 11/12] Use the I2C bus number from the board config, not a hardcoded value. --- apps/drivers/blinkm/blinkm.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 56265995f3..3fabfd9a54 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -92,6 +92,7 @@ #include +#include #include #include @@ -841,7 +842,7 @@ int blinkm_main(int argc, char *argv[]) { - int i2cdevice = 3; + int i2cdevice = PX4_I2C_BUS_EXPANSION; int blinkmadr = 9; int x; From 5514d6879ab3caae0458fe01da574d7e09b1b447 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 14:09:48 +0200 Subject: [PATCH 12/12] Docs changes --- apps/examples/kalman_demo/Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/Makefile b/apps/examples/kalman_demo/Makefile index 99c34d934e..6c592d645c 100644 --- a/apps/examples/kalman_demo/Makefile +++ b/apps/examples/kalman_demo/Makefile @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,7 +32,7 @@ ############################################################################ # -# Basic example application +# Full attitude / position Extended Kalman Filter # APPNAME = kalman_demo