diff options
author | Anton Babushkin <rk3dov@gmail.com> | 2013-04-28 17:01:44 +0400 |
---|---|---|
committer | Anton Babushkin <rk3dov@gmail.com> | 2013-04-28 17:01:44 +0400 |
commit | 29057cb3bd88dae8a2cf313888609e5f7467449e (patch) | |
tree | cff9cbe7dd13814787a39bdd673521323851ab0f /apps | |
parent | 26f9a1d42c6402a283a679e66236f247fd274cd2 (diff) | |
parent | 5514d6879ab3caae0458fe01da574d7e09b1b447 (diff) | |
download | px4-firmware-29057cb3bd88dae8a2cf313888609e5f7467449e.tar.gz px4-firmware-29057cb3bd88dae8a2cf313888609e5f7467449e.tar.bz2 px4-firmware-29057cb3bd88dae8a2cf313888609e5f7467449e.zip |
Merge branch 'master' into 6point_accel_calibration
Diffstat (limited to 'apps')
25 files changed, 86 insertions, 21 deletions
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile index 734af7cc1..46a54c660 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 index bd972f03f..1a50dde0f 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 <systemlib/perf_counter.h> #include <systemlib/err.h> +#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) { diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 56265995f..3fabfd9a5 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -92,6 +92,7 @@ #include <nuttx/config.h> +#include <arch/board/board.h> #include <drivers/device/i2c.h> #include <sys/types.h> @@ -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; diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index bb67d5e6d..cec0c49ff 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 diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index baa652d8a..ac5511e60 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/apps/examples/kalman_demo/Makefile b/apps/examples/kalman_demo/Makefile index 99c34d934..6c592d645 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 diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile index c7d212fc2..ad687958f 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/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 1dd3fc2d8..5f15facf8 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, diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile index 76cdac40d..4256a1091 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 cb1c3c618..34f058be4 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 1ed7a2fae..c98e2c0e2 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 9d0e156f6..d05493577 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 753a6843e..6f1be149c 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 aa1aa7761..a1735962e 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 d30fcba27..e54cf2f4e 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 2f3db0fdc..79a05550e 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 b016ddc57..3d8ac38cb 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 603746a20..f19cadbb6 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 0134c9948..f8bab41b6 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 f138e2640..98aadaa86 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 9609a24fd..15dd19982 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 c45775f4b..f58f9212e 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 diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 71386cba7..40d37fce2 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 4b9cfc023..d79811c0f 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.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 }, { 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 f17ae30ca..19a8239a6 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 { + 68 CCW + -136 CCW + -68 CW + 136 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]]} |