From 9315796020339906d30580892f57abcdc1238b0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 07:55:22 +0100 Subject: [PATCH 01/79] Enable S.BUS TX pin --- nuttx-configs/px4io-v2/include/board.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 4b9c906385..17e77918b4 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -103,8 +103,6 @@ #define GPIO_USART2_RTS 0xffffffff #undef GPIO_USART2_CK #define GPIO_USART2_CK 0xffffffff -#undef GPIO_USART3_TX -#define GPIO_USART3_TX 0xffffffff #undef GPIO_USART3_CK #define GPIO_USART3_CK 0xffffffff #undef GPIO_USART3_CTS From 500ac69ee46ad582eee5a4321bd53665e17032da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:13:53 +0100 Subject: [PATCH 02/79] Build test code for S.BUS output, send test characters once S.BUS1 or S.BUS2 is enabled --- src/modules/px4iofirmware/mixer.cpp | 14 ++++++++++++++ src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/sbus.c | 14 +++++++++++++- 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec6..3eaecc38b9 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,10 +254,24 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bb224f3880..0e9fee8aeb 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -218,6 +218,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus1_output(uint16_t *values, uint16_t num_values); +extern bool sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index f6ec542eb0..86240d36ab 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -93,7 +93,7 @@ int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -117,6 +117,18 @@ sbus_init(const char *device) return sbus_fd; } +bool +sbus1_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'A', 1); +} + +bool +sbus2_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'B', 1); +} + bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { From 85ec7fb40aab728ba477ffd75f48c2c731fb56fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:47:01 +0100 Subject: [PATCH 03/79] test loop --- src/modules/px4iofirmware/sbus.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 86240d36ab..39230d274c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,6 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } + ENABLE_SBUS_OUT(true); + + while (1) { + const char* hello = 'HELLO WORLD'; + if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) + break; + } + return sbus_fd; } From 3bcf34098a9fd07c0693e918396d2e3fde0fa1e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:50:44 +0100 Subject: [PATCH 04/79] Fix quotation marks --- src/modules/px4iofirmware/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 39230d274c..6608392d04 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -117,7 +117,7 @@ sbus_init(const char *device) ENABLE_SBUS_OUT(true); while (1) { - const char* hello = 'HELLO WORLD'; + const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; } From bc3f95fc07fb01edecfa9147023a354f1f237e92 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:15 -0800 Subject: [PATCH 05/79] Turn off JTAG completely in a vain attempt to get PB4 free for SBUS enable. --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 4111e70eba..2c63b88190 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -104,7 +104,7 @@ CONFIG_ARMV7M_CMNVECTOR=y # CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled # CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n CONFIG_STM32_JTAG_SW_ENABLE=n From ca2ad0051d2c5a31aa6050cc88f5c7d6c2997036 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:53 -0800 Subject: [PATCH 06/79] Be more enthusiastic with the sbus enable pin. Still no love. --- src/modules/px4iofirmware/sbus.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6608392d04..32be93d4c9 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,12 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } - ENABLE_SBUS_OUT(true); + stm32_configgpio(GPIO_SBUS_OENABLE); while (1) { + ENABLE_SBUS_OUT(true); const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; + ENABLE_SBUS_OUT(false); } return sbus_fd; From cec6b8925e727710884042f9b45c105aa8c4d5af Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:10:45 -0800 Subject: [PATCH 07/79] Don't leave all JTAG off... it will make you very sad --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 2c63b88190..e6563e5878 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_ARMV7M_CMNVECTOR=y CONFIG_STM32_DFU=n CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # # Individual subsystems can be enabled: From dd432e66032c3cb1cb6f65536c28af1dd9f97317 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:11:09 -0800 Subject: [PATCH 08/79] Remove the s.bus test loop... makes it very hard to update the firmware. --- src/modules/px4iofirmware/sbus.c | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32be93d4c9..0e7dc621cf 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -113,17 +113,6 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } - - stm32_configgpio(GPIO_SBUS_OENABLE); - - while (1) { - ENABLE_SBUS_OUT(true); - const char* hello = "HELLO WORLD"; - if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) - break; - ENABLE_SBUS_OUT(false); - } - return sbus_fd; } From 6a1f91e6254e14c52b77406b12b76e2a233aedf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:22:05 +0100 Subject: [PATCH 09/79] Make SBUS output exclusive --- src/modules/px4iofirmware/mixer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3eaecc38b9..b175c3bc8c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -255,11 +255,12 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + } } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ From 91c55503a860ffc02a2687c141e2cfc68a43b3cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:25:49 +0100 Subject: [PATCH 10/79] Ensure only either S.BUS1 or S.BUS2 can be active at a time --- src/modules/px4iofirmware/registers.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e13..f780868398 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -462,9 +462,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - /* disable the conflicting options */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif From 1362d5f195bc02f8f9c3ad0988768d547c705748 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 30 Mar 2014 20:37:28 +0400 Subject: [PATCH 11/79] px4fmu: support all actuator control groups, dynamically subscribe to required topics --- src/drivers/px4fmu/fmu.cpp | 264 ++++++++++++++++++++++--------------- 1 file changed, 161 insertions(+), 103 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c5..9a1da39cbe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,19 +120,25 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; - int _t_actuators; - int _t_actuator_armed; - orb_advert_t _t_outputs; + int _armed_sub; + orb_advert_t _outputs_pub; + actuator_armed_s _armed; unsigned _num_outputs; bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + bool _servo_armed; bool _pwm_on; MixerGroup *_mixers; - actuator_controls_s _controls; + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; @@ -149,7 +155,7 @@ private: uint8_t control_group, uint8_t control_index, float &input); - + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -216,15 +222,17 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _t_outputs(0), + _control_subs({-1}), + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(-1), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), + _servo_armed(false), _pwm_on(false), _mixers(nullptr), + _groups_required(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -235,6 +243,14 @@ PX4FMU::PX4FMU() : _max_pwm[i] = PWM_DEFAULT_MAX; } + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + _debug_enabled = true; } @@ -447,33 +463,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); } +void +PX4FMU::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + void PX4FMU::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL // rc input, published to ORB @@ -491,6 +517,12 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } /* * Adjust actuator topic update rate to keep up with @@ -515,7 +547,11 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; @@ -523,7 +559,7 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); + int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { @@ -537,90 +573,99 @@ PX4FMU::task_main() } else { - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_6PWM: - num_outputs = 6; - break; - - default: - num_outputs = 0; - break; + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - } - - uint16_t pwm_limited[num_outputs]; - - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + poll_id++; } } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = _armed.armed && !_armed.lockdown; - if (_armed != set_armed) - _armed = set_armed; + if (_servo_armed != set_armed) + _servo_armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); } } + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* publish mixed control outputs */ + if (_outputs_pub < 0) { + _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + } else { + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); + } + } } #ifdef HRT_PPM_CHANNEL @@ -661,8 +706,13 @@ PX4FMU::task_main() } - ::close(_t_actuators); - ::close(_t_actuator_armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -684,7 +734,7 @@ PX4FMU::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -1052,6 +1102,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -1060,18 +1111,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + (uintptr_t)_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -1082,9 +1135,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -1095,7 +1149,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); } } From db37d3a4959c6f0888708bae4b4efd66c668e5b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 13:36:55 +0400 Subject: [PATCH 12/79] fmu driver: bugs fixed --- src/drivers/px4fmu/fmu.cpp | 45 +++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9a1da39cbe..8aa4473d41 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -233,6 +233,7 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -584,28 +585,6 @@ PX4FMU::task_main() } } - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; - - if (_servo_armed != set_armed) - _servo_armed = set_armed; - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } - } - /* can we mix? */ if (_mixers != nullptr) { @@ -668,6 +647,28 @@ PX4FMU::task_main() } } + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + if (_servo_armed != set_armed) + _servo_armed = set_armed; + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } + } + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data From 08e5ed5a1d630f840a838006bab6b1d1ae6aa6c1 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:14:12 +0200 Subject: [PATCH 13/79] added version nsh command, it can replace hw_ver sss --- makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/version/module.mk | 43 ++++++++ src/systemcmds/version/version.c | 138 ++++++++++++++++++++++++++ 3 files changed, 182 insertions(+) create mode 100644 src/systemcmds/version/module.mk create mode 100644 src/systemcmds/version/version.c diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..7ec6a2fc68 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,6 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/version # # General system control diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/version/module.mk new file mode 100644 index 0000000000..3dac09239a --- /dev/null +++ b/src/systemcmds/version/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = version +SRCS = version.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c new file mode 100644 index 0000000000..38730b10dc --- /dev/null +++ b/src/systemcmds/version/version.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Vladimir Kulla + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file version.c + * + * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc + */ + +#include + +#include +#include +#include +#include + +__EXPORT int version_main(int argc, char *argv[]); + +static void usage(const char *reason) +{ + if (reason != NULL) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); +} + +static void version_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash:"); + } + printf("%s\n", FW_GIT); +} + +static void version_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +static void version_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +static void version_gcc(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used: "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} +static char sz_ver_hw_str[] = "hw"; + +int version_main(int argc, char *argv[]) +{ + if (argc >= 2) + { + if (argv[1] != NULL) + { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) + { + version_hwarch(0); + } + else if (!strncmp(argv[1], "git", 3)) + { + version_githash(0); + } + else if (!strncmp(argv[1], "date", 3)) + { + version_date(0); + } + else if (!strncmp(argv[1], "gcc", 3)) + { + version_gcc(0); + } + else if (!strncmp(argv[1], "all", 3)) + { + printf("Pixhawk version info\n"); + version_hwarch(1); + version_date(1); + version_githash(1); + version_gcc(1); + } + else + { + printf("unknown command: %s\n", argv[1]); + } + } + else + { + usage("Error, input parameter NULL.\n"); + } + } + else + { + usage("Error, not enough parameters."); + } + return OK; +} From fd95adc710cd04855b8f7c2abaf73d9220cd497e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:34:45 +0200 Subject: [PATCH 14/79] corrections before xmerge --- src/systemcmds/version/version.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c index 38730b10dc..37a51dbfc0 100644 --- a/src/systemcmds/version/version.c +++ b/src/systemcmds/version/version.c @@ -45,6 +45,13 @@ #include #include +static char sz_ver_hw_str[] = "hw"; +static char sz_ver_git_str[] = "git"; +static char sz_ver_date_str[] = "date"; +static char sz_ver_gcc_str[] = "gcc"; +static char sz_ver_all_str[] = "all"; + + __EXPORT int version_main(int argc, char *argv[]); static void usage(const char *reason) @@ -59,7 +66,7 @@ static void usage(const char *reason) static void version_githash(int bShowPrefix) { if (bShowPrefix == 1) { - printf("FW git-hash:"); + printf("FW git-hash: "); } printf("%s\n", FW_GIT); } @@ -88,7 +95,6 @@ static void version_gcc(int bShowPrefix) } printf("%s\n", __VERSION__); } -static char sz_ver_hw_str[] = "hw"; int version_main(int argc, char *argv[]) { @@ -100,19 +106,19 @@ int version_main(int argc, char *argv[]) { version_hwarch(0); } - else if (!strncmp(argv[1], "git", 3)) + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { version_githash(0); } - else if (!strncmp(argv[1], "date", 3)) + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { version_date(0); } - else if (!strncmp(argv[1], "gcc", 3)) + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { version_gcc(0); } - else if (!strncmp(argv[1], "all", 3)) + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { printf("Pixhawk version info\n"); version_hwarch(1); From eff15ef3f15d3fc5692b06c0669f1b758e8d744b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:08:40 +0200 Subject: [PATCH 15/79] Fix what is looking like a missing cast in CMSIS - the cast within the line would make only halfway sense if this was actually intended as double precision operation --- src/lib/mathlib/CMSIS/Include/arm_math.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h index 6f66f9ee3e..61d3a3b61f 100644 --- a/src/lib/mathlib/CMSIS/Include/arm_math.h +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h @@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32( *pIa = Ialpha; /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */ - *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; + *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; } From 5ea1105451330aa55e0c331b99ba2ab60e6f8c15 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 15:12:05 +0200 Subject: [PATCH 16/79] changed dir from version to ver to keep it shorter added "hw_ver compare" as command option so we can replace hw_ver in future --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 +- src/systemcmds/{version => ver}/module.mk | 7 +- src/systemcmds/ver/ver.c | 175 ++++++++++++++++++++++ src/systemcmds/ver/ver.h | 64 ++++++++ src/systemcmds/version/version.c | 144 ------------------ 6 files changed, 245 insertions(+), 148 deletions(-) rename src/systemcmds/{version => ver}/module.mk (90%) create mode 100644 src/systemcmds/ver/ver.c create mode 100644 src/systemcmds/ver/ver.h delete mode 100644 src/systemcmds/version/version.c diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..20e7ab3312 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -57,6 +57,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7ec6a2fc68..bbb754eb08 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,7 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile -MODULES += systemcmds/version +MODULES += systemcmds/ver # # General system control diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/ver/module.mk similarity index 90% rename from src/systemcmds/version/module.mk rename to src/systemcmds/ver/module.mk index 3dac09239a..2eeb80b616 100644 --- a/src/systemcmds/version/module.mk +++ b/src/systemcmds/ver/module.mk @@ -32,11 +32,12 @@ ############################################################################ # -# Show and test hardware version +# "version" nsh-command displays version infromation for hw,sw, gcc,build etc +# can be also included and used in own code via "ver.h" # -MODULE_COMMAND = version -SRCS = version.c +MODULE_COMMAND = ver +SRCS = ver.c MODULE_STACKSIZE = 1024 diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c new file mode 100644 index 0000000000..f44fd1afe1 --- /dev/null +++ b/src/systemcmds/ver/ver.c @@ -0,0 +1,175 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** +* @file ver.c +* +* Version command, unifies way of showing versions of HW, SW, Build, gcc +* In case you want to add new version just extend version_main function +* +* External use of the version functions is possible, include "vercmd.h" +* and use functions from header with prefix ver_ +* +* @author Vladimir Kulla +*/ + +#include +#include +#include +#include +#include + +#include "ver.h" + +// string constants for version commands +static const char sz_ver_hw_str[] = "hw"; +static const char sz_ver_hwcmp_str[]= "hwcmp"; +static const char sz_ver_git_str[] = "git"; +static const char sz_ver_date_str[] = "date"; +static const char sz_ver_gcc_str[] = "gcc"; +static const char sz_ver_all_str[] = "all"; + +static void usage(const char *reason) +{ + if (reason != NULL) { + printf("%s\n", reason); + } + + printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); +} + +void ver_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash: "); + } + printf("%s\n", FW_GIT); +} + +void ver_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +void ver_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +void ver_gcclong(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (long): "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} + +void ver_gccshort(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (short): "); + + } + printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); +} + +int ver_gccnum() +{ + return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; +} + +__EXPORT int ver_main(int argc, char *argv[]); + +int ver_main(int argc, char *argv[]) +{ + int ret = 1; //defaults to an error + + // first check if there are at least 2 params + if (argc >= 2) { + if (argv[1] != NULL) { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + ver_hwarch(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + if (argc >= 3 && argv[2] != NULL) { + // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + } else { + errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + } + } + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + ver_githash(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { + ver_date(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + ver_gcclong(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { + printf("Pixhawk version info\n"); + ver_hwarch(1); + ver_date(1); + ver_githash(1); + ver_gcclong(1); + ret = 0; + } + else { + errx(1, "unknown command.\n"); + } + } + else { + usage("Error, input parameter NULL.\n"); + } + } + else { + usage("Error, not enough parameters."); + } + + return ret; +} diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h new file mode 100644 index 0000000000..3de308c05c --- /dev/null +++ b/src/systemcmds/ver/ver.h @@ -0,0 +1,64 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** +* @file vercmd.h +* +* Version command, unifies way of showing versions of hW, sW, build, gcc +* Mainly used from nsh-console, shall replace also hw_arch command +* +* External use: possible, include "vercmd.h" and use "ver_xx" functions +* +* Extension: possible, just add new else-if to version_main function +* +* @author Vladimir Kulla +*/ + +#ifndef VERCMD_H_ +#define VERCMD_H_ + + +void ver_githash(int bShowPrefix); + +void ver_hwarch(int bShowPrefix); + +void ver_date(int bShowPrefix); + +void ver_gcclong(int bShowPrefix); +void ver_gccshort(int bShowPrefix); +int ver_gccnum(); + + + + +#endif /* VERCMD_H_ */ diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c deleted file mode 100644 index 37a51dbfc0..0000000000 --- a/src/systemcmds/version/version.c +++ /dev/null @@ -1,144 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Vladimir Kulla - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file version.c - * - * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc - */ - -#include - -#include -#include -#include -#include - -static char sz_ver_hw_str[] = "hw"; -static char sz_ver_git_str[] = "git"; -static char sz_ver_date_str[] = "date"; -static char sz_ver_gcc_str[] = "gcc"; -static char sz_ver_all_str[] = "all"; - - -__EXPORT int version_main(int argc, char *argv[]); - -static void usage(const char *reason) -{ - if (reason != NULL) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); -} - -static void version_githash(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("FW git-hash: "); - } - printf("%s\n", FW_GIT); -} - -static void version_hwarch(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("HW arch: "); - } - printf("%s\n", HW_ARCH); -} - -static void version_date(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("Build date: "); - } - printf("%s %s\n", __DATE__, __TIME__); -} - -static void version_gcc(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used: "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); - } - printf("%s\n", __VERSION__); -} - -int version_main(int argc, char *argv[]) -{ - if (argc >= 2) - { - if (argv[1] != NULL) - { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) - { - version_hwarch(0); - } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) - { - version_githash(0); - } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) - { - version_date(0); - } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) - { - version_gcc(0); - } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) - { - printf("Pixhawk version info\n"); - version_hwarch(1); - version_date(1); - version_githash(1); - version_gcc(1); - } - else - { - printf("unknown command: %s\n", argv[1]); - } - } - else - { - usage("Error, input parameter NULL.\n"); - } - } - else - { - usage("Error, not enough parameters."); - } - return OK; -} From e6d332aa7cd038b00643a70d99e82d60b7ffbcf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 15:50:53 +0200 Subject: [PATCH 17/79] Make commander less pedantic about position status --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..53ed34f46b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 08408594ec3f136bdcf554a2a75c2e0af8e03c8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 16:06:34 +0200 Subject: [PATCH 18/79] Renamed parameters which changed from RAD to DEGREES to avoid user confusion. Also made naming of the two parameters more consistent. --- .../mc_pos_control/mc_pos_control_params.c | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index fe0004ea8a..104df4e59b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -174,16 +174,28 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** - * Maximum tilt + * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes. + * Limits maximum tilt in AUTO and EASY modes during flight. * * @unit deg * @min 0.0 * @max 90.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); + +/** + * Maximum tilt during landing + * + * Limits maximum tilt angle on landing. + * + * @unit deg + * @min 0.0 + * @max 90.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 15.0f); /** * Landing descend rate @@ -194,14 +206,3 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); -/** - * Maximum landing tilt - * - * Limits maximum tilt on landing. - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 15.0f); From bd5a0cef1aad42f8deb1d58e573631436630246e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 17:42:45 +0200 Subject: [PATCH 19/79] ver command ready including hwcmp which replaces hw_ver, removed hw_ver updated all scripts to use new ver hwcmp command q --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 +-- makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 2 +- src/systemcmds/hw_ver/hw_ver.c | 73 --------------------------- src/systemcmds/hw_ver/module.mk | 43 ---------------- src/systemcmds/ver/ver.c | 64 ++++++++--------------- src/systemcmds/ver/ver.h | 64 ----------------------- 11 files changed, 29 insertions(+), 233 deletions(-) delete mode 100644 src/systemcmds/hw_ver/hw_ver.c delete mode 100644 src/systemcmds/hw_ver/module.mk delete mode 100644 src/systemcmds/ver/ver.h diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 6e8fdbc0f7..e23aebd87f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ px4io recovery # Adjust PX4IO update rate limit # set PX4IO_LIMIT 400 -if hw_ver compare PX4FMU_V1 +if ver hwcmp PX4FMU_V1 then set PX4IO_LIMIT 200 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273c..3469d5f5f5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,7 +5,7 @@ if [ -d /fs/microsd ] then - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 8 -t diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 235be24316..1e14930fe3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -22,7 +22,7 @@ then echo "[init] Using L3GD20(H)" fi -if hw_ver compare PX4FMU_V2 +if ver hwcmp PX4FMU_V2 then if lsm303d start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea5cf8deb7..3fb5fb7333 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -246,7 +246,7 @@ then echo "[init] ERROR: PX4IO not found, disabling output" # Avoid using ttyS0 for MAVLink on FMUv1 - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -260,7 +260,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -306,7 +306,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then @@ -381,7 +381,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 20e7ab3312..61a417f308 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -55,7 +55,6 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bbb754eb08..65ca24325c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,7 +63,6 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d7..84274bf754 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -29,7 +29,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver +MODULES += systemcmds/ver # # Library modules diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c deleted file mode 100644 index 4b84523cc1..0000000000 --- a/src/systemcmds/hw_ver/hw_ver.c +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hw_ver.c - * - * Show and test hardware version. - */ - -#include - -#include -#include -#include -#include - -__EXPORT int hw_ver_main(int argc, char *argv[]); - -int -hw_ver_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "show")) { - printf(HW_ARCH "\n"); - exit(0); - } - - if (!strcmp(argv[1], "compare")) { - if (argc >= 3) { - int ret = strcmp(HW_ARCH, argv[2]) != 0; - if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); - } - exit(ret); - - } else { - errx(1, "not enough arguments, try 'compare PX4FMU_1'"); - } - } - } - - errx(1, "expected a command, try 'show' or 'compare'"); -} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk deleted file mode 100644 index 3cc08b6a1a..0000000000 --- a/src/systemcmds/hw_ver/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Show and test hardware version -# - -MODULE_COMMAND = hw_ver -SRCS = hw_ver.c - -MODULE_STACKSIZE = 1024 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index f44fd1afe1..c932bc1624 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -34,28 +34,22 @@ /** * @file ver.c * -* Version command, unifies way of showing versions of HW, SW, Build, gcc +* Version command, unifies way of showing versions of HW, SW, Build, GCC * In case you want to add new version just extend version_main function * -* External use of the version functions is possible, include "vercmd.h" -* and use functions from header with prefix ver_ -* * @author Vladimir Kulla */ -#include #include #include -#include #include - -#include "ver.h" +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[]= "hwcmp"; static const char sz_ver_git_str[] = "git"; -static const char sz_ver_date_str[] = "date"; +static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; @@ -65,10 +59,10 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); } -void ver_githash(int bShowPrefix) +static void ver_githash(int bShowPrefix) { if (bShowPrefix == 1) { printf("FW git-hash: "); @@ -76,7 +70,7 @@ void ver_githash(int bShowPrefix) printf("%s\n", FW_GIT); } -void ver_hwarch(int bShowPrefix) +static void ver_hwarch(int bShowPrefix) { if (bShowPrefix == 1) { printf("HW arch: "); @@ -84,37 +78,22 @@ void ver_hwarch(int bShowPrefix) printf("%s\n", HW_ARCH); } -void ver_date(int bShowPrefix) +static void ver_bdate(int bShowPrefix) { if (bShowPrefix == 1) { - printf("Build date: "); + printf("Build datetime: "); } printf("%s %s\n", __DATE__, __TIME__); } -void ver_gcclong(int bShowPrefix) +static void ver_gcclong(int bShowPrefix) { if (bShowPrefix == 1) { - printf("GCC used (long): "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + printf("GCC toolchain: "); } printf("%s\n", __VERSION__); } -void ver_gccshort(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used (short): "); - - } - printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); -} - -int ver_gccnum() -{ - return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; -} - __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) @@ -124,37 +103,36 @@ int ver_main(int argc, char *argv[]) // first check if there are at least 2 params if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { ver_hwarch(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { // compare 3rd parameter with HW_ARCH string, in case of match, return 0 - ret = strcmp(HW_ARCH, argv[2]) != 0; + ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); + printf("hwver match: %s\n", HW_ARCH); } } else { - errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { ver_githash(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { - ver_date(0); + else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + ver_bdate(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { ver_gcclong(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { - printf("Pixhawk version info\n"); + else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { ver_hwarch(1); - ver_date(1); + ver_bdate(1); ver_githash(1); ver_gcclong(1); ret = 0; diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h deleted file mode 100644 index 3de308c05c..0000000000 --- a/src/systemcmds/ver/ver.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2012-2014 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 -* are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in -* the documentation and/or other materials provided with the -* distribution. -* 3. Neither the name PX4 nor the names of its contributors may be -* used to endorse or promote products derived from this software -* without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -****************************************************************************/ - -/** -* @file vercmd.h -* -* Version command, unifies way of showing versions of hW, sW, build, gcc -* Mainly used from nsh-console, shall replace also hw_arch command -* -* External use: possible, include "vercmd.h" and use "ver_xx" functions -* -* Extension: possible, just add new else-if to version_main function -* -* @author Vladimir Kulla -*/ - -#ifndef VERCMD_H_ -#define VERCMD_H_ - - -void ver_githash(int bShowPrefix); - -void ver_hwarch(int bShowPrefix); - -void ver_date(int bShowPrefix); - -void ver_gcclong(int bShowPrefix); -void ver_gccshort(int bShowPrefix); -int ver_gccnum(); - - - - -#endif /* VERCMD_H_ */ From ab257ebcced2af6ddb528a9d48355dc2cac7d10a Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Mon, 28 Apr 2014 00:52:19 -0400 Subject: [PATCH 20/79] Proper data manager restart handling Introduce SYS_RESTART_TYPE parameter having one of 3 values: boot restart, inflight restart, or unknown restart, and defaulting to unknown restart. px4io.cpp sets this parameter according to the type of restart detected. dataman.c retrieves this parameter and clears data entries according to their persistence level. Does nothing if unknown restart. --- src/drivers/px4io/px4io.cpp | 17 ++++++++++++++ src/modules/dataman/dataman.c | 33 ++++++++++++++++++++------- src/modules/dataman/dataman.h | 5 ++-- src/modules/systemlib/system_params.c | 27 +++++++++++++++------- 4 files changed, 64 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e318e206ad..ba13d0ff72 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -91,6 +91,8 @@ #include "uploader.h" +#include "modules/dataman/dataman.h" + extern device::Device *PX4IO_i2c_interface() weak_function; extern device::Device *PX4IO_serial_interface() weak_function; @@ -568,9 +570,15 @@ int PX4IO::init() { int ret; + param_t sys_restart_param; + int sys_restart_val = DM_INIT_REASON_VOLATILE; ASSERT(_task == -1); + sys_restart_param = param_find("SYS_RESTART_TYPE"); + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + /* do regular cdev init */ ret = CDev::init(); @@ -720,6 +728,11 @@ PX4IO::init() /* keep waiting for state change for 2 s */ } while (!safety.armed); + /* Indicate restart type is in-flight */ + sys_restart_val = DM_INIT_REASON_IN_FLIGHT; + param_set(sys_restart_param, &sys_restart_val); + + /* regular boot, no in-air restart, init IO */ } else { @@ -745,6 +758,10 @@ PX4IO::init() } } + /* Indicate restart type is power on */ + sys_restart_val = DM_INIT_REASON_POWER_ON; + param_set(sys_restart_param, &sys_restart_val); + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index c132b0040b..7505ba3584 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -416,26 +416,26 @@ static int _restart(dm_reset_reason reason) { unsigned char buffer[2]; - int offset, result = 0; + int offset = 0, result = 0; /* We need to scan the entire file and invalidate and data that should not persist after the last reset */ /* Loop through all of the data segments and delete those that are not persistent */ - offset = 0; - while (1) { size_t len; /* Get data segment at current offset */ if (lseek(g_task_fd, offset, SEEK_SET) != offset) { - result = -1; + /* must be at eof */ break; } len = read(g_task_fd, buffer, sizeof(buffer)); - if (len == 0) + if (len != sizeof(buffer)) { + /* must be at eof */ break; + } /* check if segment contains data */ if (buffer[0]) { @@ -443,12 +443,12 @@ _restart(dm_reset_reason reason) /* Whether data gets deleted depends on reset type and data segment's persistence setting */ if (reason == DM_INIT_REASON_POWER_ON) { - if (buffer[1] != DM_PERSIST_POWER_ON_RESET) { + if (buffer[1] > DM_PERSIST_POWER_ON_RESET) { clear_entry = 1; } } else { - if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) { + if (buffer[1] > DM_PERSIST_IN_FLIGHT_RESET) { clear_entry = 1; } } @@ -628,6 +628,23 @@ task_main(int argc, char *argv[]) fsync(g_task_fd); + /* see if we need to erase any items based on restart type */ + int sys_restart_val; + if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) { + if (sys_restart_val == DM_INIT_REASON_POWER_ON) { + warnx("Power on restart"); + _restart(DM_INIT_REASON_POWER_ON); + } + else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + warnx("In flight restart"); + _restart(DM_INIT_REASON_IN_FLIGHT); + } + else + warnx("Unknown restart"); + } + else + warnx("Unknown restart"); + /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ /* worker thread is shutting down but still processing requests */ @@ -724,7 +741,7 @@ start(void) return -1; } - /* wait for the thread to actuall initialize */ + /* wait for the thread to actually initialize */ sem_wait(&g_init_sema); sem_destroy(&g_init_sema); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 33c9fcd157..4382baeb58 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -75,7 +75,8 @@ extern "C" { /* The reason for the last reset */ typedef enum { DM_INIT_REASON_POWER_ON = 0, /* Data survives resets */ - DM_INIT_REASON_IN_FLIGHT /* Data survives in-flight resets only */ + DM_INIT_REASON_IN_FLIGHT, /* Data survives in-flight resets only */ + DM_INIT_REASON_VOLATILE /* Data does not survive reset */ } dm_reset_reason; /* Maximum size in bytes of a single item instance */ @@ -100,7 +101,7 @@ extern "C" { size_t buflen /* Length in bytes of data to retrieve */ ); - /* Retrieve from the data manager store */ + /* Erase all items of this type */ __EXPORT int dm_clear( dm_item_t item /* The item type to clear */ diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index ec2bc3a9a0..702e435ac9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -62,12 +62,23 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); /** - * Set usage of IO board - * - * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. - * - * @min 0 - * @max 1 - * @group System - */ +* Set usage of IO board +* +* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. +* +* @min 0 +* @max 1 +* @group System +*/ PARAM_DEFINE_INT32(SYS_USE_IO, 1); + +/** +* Set restart type +* +* Set by px4io to indicate type of restart +* +* @min 0 +* @max 2 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); From 0745334ac46c40ed407dc11ebbb7c252e37bcb43 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 11:41:40 +0200 Subject: [PATCH 21/79] Reset init flags as well --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..6eccc4c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,6 +2407,19 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // Clear the init flag statesInitialised = false; + // Clear other flags, waiting for new data + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + // onGround(true), + // staticMode(true), + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements From e9c94fa58113f60c82be6d829fadd307195ecfb7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Apr 2014 21:11:53 +1000 Subject: [PATCH 22/79] Debug: fixes for Nuttx.py debug gdb add-ons also fixed preceding mask calculation in show heaps --- Debug/Nuttx.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index 093edd0e09..7cc21b99f4 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -306,12 +306,15 @@ class NX_show_heap (gdb.Command): def __init__(self): super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) - if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8: - self._allocflag = 0x80000000 - self._allocnodesize = 8 - else: + struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') + preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof + if preceding_size == 2: self._allocflag = 0x8000 - self._allocnodesize = 4 + elif preceding_size == 4: + self._allocflag = 0x80000000 + else: + raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) + self._allocnodesize = struct_mm_allocnode_s.sizeof def _node_allocated(self, allocnode): if allocnode['preceding'] & self._allocflag: @@ -333,7 +336,8 @@ class NX_show_heap (gdb.Command): state = '' else: state = '(free)' - print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state) + print ' {} {} {}'.format(allocnode.address + self._allocnodesize, + self._node_size(allocnode), state) cursor += self._node_size(allocnode) / self._allocnodesize def invoke(self, args, from_tty): From f1258da61086b82ad04a9945dd36d52735efc6fb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 26 Apr 2014 21:12:06 +1000 Subject: [PATCH 23/79] Debug: fixes for gdb extension macros --- Debug/NuttX | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Debug/NuttX b/Debug/NuttX index 3b95e96b2f..d34e9f5b4b 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -34,10 +34,10 @@ define _showheap else set $MM_ALLOC_BIT = 0x80000000 end - printf "HEAP %d %p - %p\n", $index, g_heapstart[$index], g_heapend[$index] + printf "HEAP %d %p - %p\n", $index, g_mmheap.mm_heapstart[$index], g_mmheap.mm_heapend[$index] printf "ptr size\n" - set $node = (char *)g_heapstart[$index] + sizeof(struct mm_allocnode_s) - while $node < g_heapend[$index] + set $node = (char *)g_mmheap.mm_heapstart[$index] + sizeof(struct mm_allocnode_s) + while $node < g_mmheap.mm_heapend[$index] printf " %p", $node set $nodestruct = (struct mm_allocnode_s *)$node printf " %u", $nodestruct->size @@ -47,7 +47,7 @@ define _showheap else set $used = $used + $nodestruct->size end - if ($nodestruct->size > g_heapsize) || (($node + $nodestruct->size) > g_heapend[$index]) + if ($nodestruct->size > g_mmheap.mm_heapsize) || (($node + $nodestruct->size) > g_mmheap.mm_heapend[$index]) printf " (BAD SIZE)" end printf "\n" @@ -59,7 +59,7 @@ define _showheap end define showheap - set $nheaps = sizeof(g_heapstart) / sizeof(g_heapstart[0]) + set $nheaps = sizeof(g_mmheap.mm_heapstart) / sizeof(g_mmheap.mm_heapstart[0]) printf "Printing %d heaps\n", $nheaps set $heapindex = (int)0 while $heapindex < $nheaps From 9f2c4b7513adb6c543fd2c0f729f11ed3d195f72 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 3 Apr 2014 22:09:24 +0900 Subject: [PATCH 24/79] tone_alarm: add PARACHUTE_RELEASE_TUNE --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 147d7123a4..b7981e9c4b 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -148,6 +148,7 @@ enum { TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, + TONE_PARACHUTE_RELEASE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 8ed0de58c6..810f4aacc4 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -335,6 +335,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Tue, 11 Feb 2014 15:55:40 +1100 Subject: [PATCH 25/79] px4io: support PX4IO_P_SETUP_FORCE_SAFETY_OFF this allows the FMU to force the safety off on the IO board. Useful in two cases: 1) vehicles where the safety switch is impractical or not useful (eg. HAB planes or internal combustion motors) 2) doing ESC calibration on multi-copters --- src/modules/px4iofirmware/protocol.h | 4 ++++ src/modules/px4iofirmware/registers.c | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a978d483a4..6c20d6006d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -211,6 +211,10 @@ enum { /* DSM bind states */ /* 12 occupied by CRC */ #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ +#define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into + 'armed' (PWM enabled) state */ +#define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ #define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6752e7b4b3..9e5d7e7e20 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -570,6 +570,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) dsm_bind(value & 0x0f, (value >> 4) & 0xF); break; + case PX4IO_P_SETUP_FORCE_SAFETY_OFF: + if (value == PX4IO_FORCE_SAFETY_MAGIC) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } + break; + default: return -1; } From ab1939c6a30b6f4de06c83245c9f99ed350a4559 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 25 Mar 2014 11:54:37 +1100 Subject: [PATCH 26/79] pwm: added PWM_SERVO_SET_FORCE_SAFETY_OFF ioctl this allows the safety switch on px4io to be forced off --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4fmu/fmu.cpp | 1 + src/drivers/px4io/px4io.cpp | 4 ++++ 3 files changed, 8 insertions(+) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 7a93e513ed..972573f9ff 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm); /** get the lockdown override flag to enable outputs in HIL */ #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) +/** force safety switch off (to disable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) + /* * * diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c5..7258d5f9e6 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -736,6 +736,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: // these are no-ops, as no safety switch break; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e318e206ad..e5a39ffb0b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2129,6 +2129,10 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + /* force safety swith off */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; case DSM_BIND_START: From b0b2f714f19a48d3823c2ddf4d82662150a58a7e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Apr 2014 15:23:56 +0200 Subject: [PATCH 27/79] add options do disable or fake gps output in rcS --- ROMFS/px4fmu_common/init.d/rcS | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea5cf8deb7..c12d876cb7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -120,6 +120,8 @@ then set EXIT_ON_END no set MAV_TYPE none set LOAD_DEFAULT_APPS yes + set GPS yes + set GPS_FAKE no # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -437,9 +439,20 @@ then echo "[init] Start logging" sh /etc/init.d/rc.logging fi - - gps start + if [ $GPS == yes ] + then + echo "[init] Start GPS" + if [ $GPS_FAKE == yes ] + then + echo "[init] Faking GPS" + gps start -f + else + gps start + fi + fi + + # # Start up ARDrone Motor interface # From 6a2ecfa162fe49bf5c5cef0100035c3ca270dc2f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 28 Apr 2014 15:57:14 +0200 Subject: [PATCH 28/79] remove whitespace --- ROMFS/px4fmu_common/init.d/rcS | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c12d876cb7..492afcb4c1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -452,7 +452,6 @@ then fi fi - # # Start up ARDrone Motor interface # From e134537ae8f892ba3ae4a2f9c771bcfa62f905c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 17:44:29 +0200 Subject: [PATCH 29/79] Added automatic declination lookup --- src/lib/geo/geo.h | 2 + src/lib/geo/geo_mag_declination.c | 135 ++++++++++++++++++++++++++++++ src/lib/geo/geo_mag_declination.h | 47 +++++++++++ src/lib/geo/module.mk | 3 +- 4 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/lib/geo/geo_mag_declination.c create mode 100644 src/lib/geo/geo_mag_declination.h diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d970..d987afe337 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,6 +48,8 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" +#include "geo/geo_mag_declination.h" + __BEGIN_DECLS #include diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c new file mode 100644 index 0000000000..7b4aa69a2f --- /dev/null +++ b/src/lib/geo/geo_mag_declination.c @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10 +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const char declination_table[13][37] = \ +{ + 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ + -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ + -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ + 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ + -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ + 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ + 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ + -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ + -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ + 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ + 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ + 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ + 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ + 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ + -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ + 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ + 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ + 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +__EXPORT float get_mag_declination(float lat, float lon) +{ + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} \ No newline at end of file diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h new file mode 100644 index 0000000000..0ac062d6d4 --- /dev/null +++ b/src/lib/geo/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +__EXPORT float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 30a2dc99fd..9500a2bccf 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -35,4 +35,5 @@ # Geo library # -SRCS = geo.c +SRCS = geo.c \ + geo_mag_declination.c From 4378454a100c93b4e4f93266dbe626aa540a88d3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 17:49:57 +0200 Subject: [PATCH 30/79] mc_pos_control: hotfix, MPC_TILTMAX_AIR and MPC_TILTMAX_LND parameters fixed --- .../mc_pos_control/mc_pos_control_main.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 65f4cbeaa1..7c625a0c53 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -148,17 +148,17 @@ private: param_t xy_vel_d; param_t xy_vel_max; param_t xy_ff; - param_t tilt_max; + param_t tilt_max_air; param_t land_speed; - param_t land_tilt_max; + param_t tilt_max_land; } _params_handles; /**< handles for interesting parameters */ struct { float thr_min; float thr_max; - float tilt_max; + float tilt_max_air; float land_speed; - float land_tilt_max; + float tilt_max_land; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -308,9 +308,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); _params_handles.xy_ff = param_find("MPC_XY_FF"); - _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); - _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); + _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); /* fetch initial parameter values */ parameters_update(true); @@ -355,11 +355,11 @@ MulticopterPositionControl::parameters_update(bool force) if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); - param_get(_params_handles.tilt_max, &_params.tilt_max); - _params.tilt_max = math::radians(_params.tilt_max); + param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); + _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); - param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); - _params.land_tilt_max = math::radians(_params.land_tilt_max); + param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); + _params.tilt_max_land = math::radians(_params.tilt_max_land); float v; param_get(_params_handles.xy_p, &v); @@ -841,13 +841,13 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } - float tilt_max = _params.tilt_max; + float tilt_max = _params.tilt_max_air; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ - tilt_max = _params.land_tilt_max; + tilt_max = _params.tilt_max_land; if (thr_min < 0.0f) { thr_min = 0.0f; From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: [PATCH 31/79] Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/estimator.cpp | 33 +++++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 7 ++-- .../fw_att_pos_estimator_main.cpp | 13 +++++--- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 8ac2d61541..1ca34ec307 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() : gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f) + VtasMeas(0.0f), + magDeclination(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED); + InitializeDynamic(velNED, magDeclination); return 1; } @@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound() return 0; } -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f magY = my * cosRoll - mz * sinRoll; initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg -= declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; } -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { // Clear the init flag @@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); + magDeclination = declination; + // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states @@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) magstate.q1 = initQuat[1]; magstate.q2 = initQuat[2]; magstate.q3 = initQuat[3]; - magstate.magN = magData.x; - magstate.magE = magData.y; - magstate.magD = magData.z; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; magstate.magXbias = magBias.x; magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; @@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { //store initial lat,long and height latRef = referenceLat; @@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED); + InitializeDynamic(initvelNED, declination); } void AttPosEKF::ZeroVariables() diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e118ae5734..378107b698 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -208,6 +208,7 @@ public: float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -309,7 +310,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); float ConstrainFloat(float val, float min, float max); @@ -334,7 +335,7 @@ void GetLastErrorState(struct ekf_status_report *last_error); bool StatesNaN(struct ekf_status_report *err_report); void FillErrorReport(struct ekf_status_report *err); -void InitializeDynamic(float (&initvelNED)[3]); +void InitializeDynamic(float (&initvelNED)[3], float declination); protected: @@ -342,7 +343,7 @@ bool FilterHealthy(); void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d7044..23f9e80bd8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: [PATCH 32/79] ekf_att_pos_estimator: local position reference fixed --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd8..fb019a354e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; From c179863b1e35628a75efef624d2df0b0e85ad923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:20:54 -0800 Subject: [PATCH 33/79] Improve testing infrastructure for mixer and SBUS2 --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 57 +++++++++---------------- Tools/tests-host/board_config.h | 0 Tools/tests-host/debug.h | 5 +++ Tools/tests-host/run_tests.sh | 6 +++ Tools/tests-host/sbus2_test.cpp | 75 +++++++++++++++++++++++++++++++++ 6 files changed, 108 insertions(+), 36 deletions(-) create mode 100644 Tools/tests-host/board_config.h create mode 100644 Tools/tests-host/debug.h create mode 100755 Tools/tests-host/run_tests.sh create mode 100644 Tools/tests-host/sbus2_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 61e0915515..1618faf581 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,2 +1,3 @@ ./obj/* mixer_test +sbus2_test \ No newline at end of file diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 7ab1454f01..15ccf15945 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,47 +1,32 @@ CC=g++ -CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ + -I../../src -D__EXPORT="" -Dnullptr="0" -lm -ODIR=obj -LDIR =../lib +all: mixer_test sbus2_test -LIBS=-lm +MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ + ../../src/systemcmds/tests/test_conv.cpp \ + ../../src/modules/systemlib/mixer/mixer_simple.cpp \ + ../../src/modules/systemlib/mixer/mixer_multirotor.cpp \ + ../../src/modules/systemlib/mixer/mixer.cpp \ + ../../src/modules/systemlib/mixer/mixer_group.cpp \ + ../../src/modules/systemlib/mixer/mixer_load.c \ + ../../src/modules/systemlib/pwm_limit/pwm_limit.c \ + hrt.cpp \ + mixer_test.cpp -#_DEPS = test.h -#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) +SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ + hrt.cpp \ + sbus2_test.cpp -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ - mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o -OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) +mixer_test: $(MIXER_FILES) + $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) -#$(DEPS) -$(ODIR)/%.o: %.cpp - mkdir -p obj - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c - $(CC) -c -o $@ $< $(CFLAGS) - -# -mixer_test: $(OBJ) - g++ -o $@ $^ $(CFLAGS) $(LIBS) +sbus2_test: $(SBUS2_FILES) + $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test \ No newline at end of file diff --git a/Tools/tests-host/board_config.h b/Tools/tests-host/board_config.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/debug.h b/Tools/tests-host/debug.h new file mode 100644 index 0000000000..9824d13fc3 --- /dev/null +++ b/Tools/tests-host/debug.h @@ -0,0 +1,5 @@ + +#pragma once + +#include +#define lowsyslog warnx \ No newline at end of file diff --git a/Tools/tests-host/run_tests.sh b/Tools/tests-host/run_tests.sh new file mode 100755 index 0000000000..ff5ee509ac --- /dev/null +++ b/Tools/tests-host/run_tests.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +make clean +make all +./mixer_test +./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt \ No newline at end of file diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp new file mode 100644 index 0000000000..e3b0122267 --- /dev/null +++ b/Tools/tests-host/sbus2_test.cpp @@ -0,0 +1,75 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("SBUS2 test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + // Init the parser + uint8_t frame[30]; + unsigned partial_frame_count = 0; + uint16_t rc_values[18]; + uint16_t num_values; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + partial_frame_count = 0; + warnx("FRAME RESET\n\n"); + } + + frame[partial_frame_count] = x; + partial_frame_count++; + + //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); + + if (partial_frame_count == sizeof(frame)) + partial_frame_count = 0; + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + if (partial_frame_count % 25 == 0) + sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +} \ No newline at end of file From db304910510ed2ae8c262097fa6e8df1db965d5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:20:14 +0200 Subject: [PATCH 34/79] Add missing newline --- Tools/tests-host/.gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 1618faf581..a6ce91d694 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,3 @@ ./obj/* mixer_test -sbus2_test \ No newline at end of file +sbus2_test From 002ff7da7e792010c4eba5903075af83cb1ebd3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:21:13 -0800 Subject: [PATCH 35/79] Add missing header in HRT --- src/drivers/drv_hrt.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d130d68b34..8bfc90c64b 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,6 +41,7 @@ #include #include +#include #include #include From 3959d0c1c9cf32ac1a2fa4df63fa0f0f77cc17a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:24:44 +0200 Subject: [PATCH 36/79] Disable sbus2_test for now, just leverage the testing infrastructure --- Tools/tests-host/sbus2_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index e3b0122267..134a71b809 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) { hrt_abstime now = hrt_absolute_time(); if (partial_frame_count % 25 == 0) - sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } if (ret == EOF) { From da525f29f13e5bdae717fbf189c5a3b4ab46794c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:25:13 +0200 Subject: [PATCH 37/79] Add missing header in mixer load command --- src/modules/systemlib/mixer/mixer_load.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index b05273c0de..bf3428a50b 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -41,6 +41,7 @@ #include #include #include +#include #include "mixer_load.h" From 238a3636faae13c4ba52ab9581a305c7a069276e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:13 +0200 Subject: [PATCH 38/79] Add autodeclination --- Tools/tests-host/.gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index a6ce91d694..87b314c614 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,4 @@ ./obj/* mixer_test sbus2_test +autodeclination_test From 7aefcb7a09a12fae2dcbe2bc2360948aefbb66a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:45 +0200 Subject: [PATCH 39/79] Add autodeclination testing - no-op right now --- Tools/tests-host/Makefile | 11 +++++++++-- Tools/tests-host/autodeclination_test.cpp | 19 +++++++++++++++++++ Tools/tests-host/sbus2_test.cpp | 2 +- 3 files changed, 29 insertions(+), 3 deletions(-) create mode 100644 Tools/tests-host/autodeclination_test.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 15ccf15945..fd001e4d7f 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test +all: mixer_test sbus2_test autodeclination_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,13 +20,20 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ + hrt.cpp \ + autodeclination_test.cpp + mixer_test: $(MIXER_FILES) $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +autodeclination_test: $(SBUS2_FILES) + $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp new file mode 100644 index 0000000000..b1b30f5d97 --- /dev/null +++ b/Tools/tests-host/autodeclination_test.cpp @@ -0,0 +1,19 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("autodeclination test started"); + + if (argc < 3) + errx(1, "Need lat/lon!"); + + + +} \ No newline at end of file diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index 134a71b809..281903cf68 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -62,7 +62,7 @@ int main(int argc, char *argv[]) { // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - if (partial_frame_count % 25 == 0) + //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } From 9c81ab113e73c8862cc3f4e81411cb69dbec14ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:42:46 +0200 Subject: [PATCH 40/79] Updated outo-test --- Tools/tests-host/Makefile | 6 +++--- Tools/tests-host/autodeclination_test.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index fd001e4d7f..f0737ef88c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ - -I../../src -D__EXPORT="" -Dnullptr="0" -lm + -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm all: mixer_test sbus2_test autodeclination_test @@ -20,7 +20,7 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp -AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -31,7 +31,7 @@ sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) autodeclination_test: $(SBUS2_FILES) - $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp index b1b30f5d97..6c751dc1e7 100644 --- a/Tools/tests-host/autodeclination_test.cpp +++ b/Tools/tests-host/autodeclination_test.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -7,6 +8,7 @@ #include #include #include "../../src/systemcmds/tests/tests.h" +#include int main(int argc, char *argv[]) { warnx("autodeclination test started"); @@ -14,6 +16,13 @@ int main(int argc, char *argv[]) { if (argc < 3) errx(1, "Need lat/lon!"); - + char* p_end; + + float lat = strtod(argv[1], &p_end); + float lon = strtod(argv[2], &p_end); + + float declination = get_mag_declination(lat, lon); + + printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination); } \ No newline at end of file From ec50f73cbe4c88a57f92f888d764a678f6796dd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:44:11 +0200 Subject: [PATCH 41/79] Updated geo lib C/C++ interfacing --- src/lib/geo/geo.h | 4 ++-- src/lib/geo/geo_mag_declination.c | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index d987afe337..e2f3da6f80 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,10 +48,10 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" -#include "geo/geo_mag_declination.h" - __BEGIN_DECLS +#include "geo/geo_mag_declination.h" + #include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index 7b4aa69a2f..b96f877214 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -43,6 +43,8 @@ * */ +#include + /** set this always to the sampling in degrees for the table below */ #define SAMPLING_RES 10 From 0c58588a87697a371249b7eb5ca9cdac7169c6e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 29 Apr 2014 19:51:05 +0200 Subject: [PATCH 42/79] mc_att_control: yaw feed-forward in manual control modes fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 6b0c44fb31..36d95bf063 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -496,7 +496,8 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt); + yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; } From 848c1c2552996281685d2691c1b394f25a4391b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 29 Apr 2014 21:42:58 +0200 Subject: [PATCH 43/79] ekf: More complete re-initialization --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1ca34ec307..e1ffc15d3b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2536,6 +2536,8 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); + magBias.zero(); + magState.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From d54f9973f26e5503f33ad53fd927696089a742ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:13:42 +0200 Subject: [PATCH 44/79] Fixed typos in declination table lookup --- src/lib/geo/geo_mag_declination.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f877214..cfee6b423d 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -115,8 +115,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); From e2af77123fb14e9e19d1aa9e02f04b62a342c724 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:25:36 +0200 Subject: [PATCH 45/79] ekf: Cleanup init --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index e1ffc15d3b..a294210459 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2455,7 +2455,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) Mat3f DCM; quat2Tbn(DCM, initQuat); Vector3f initMagNED; - initMagXYZ = magData - magBias; initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; @@ -2497,13 +2496,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) //Define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, latRef); - //Initialise summed variables used by covariance prediction - summedDelAng.x = 0.0f; - summedDelAng.y = 0.0f; - summedDelAng.z = 0.0f; - summedDelVel.x = 0.0f; - summedDelVel.y = 0.0f; - summedDelVel.z = 0.0f; } void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) @@ -2537,7 +2529,7 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); magBias.zero(); - magState.zero(); + magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From 48a9ba39afec8443ad4cb7d34f42e0cfc37d4e1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:13:42 +0200 Subject: [PATCH 46/79] Fixed typos in declination table lookup --- src/lib/geo/geo_mag_declination.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f877214..cfee6b423d 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -115,8 +115,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); From be1f39e3851036e43be1bea0fa09096691559a0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:52:37 +0200 Subject: [PATCH 47/79] ekf Print declination on init --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fb019a354e..b87338282a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1013,7 +1013,7 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; From 5429b82ae0fe777e31a1a52fa98f472f343e33af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:53:22 +0200 Subject: [PATCH 48/79] Fix last data type and casting compiler nuisances --- src/lib/geo/geo_mag_declination.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index cfee6b423d..09eac38f44 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -45,15 +45,14 @@ #include - /** set this always to the sampling in degrees for the table below */ -#define SAMPLING_RES 10 +#define SAMPLING_RES 10.0f #define SAMPLING_MIN_LAT -60.0f #define SAMPLING_MAX_LAT 60.0f #define SAMPLING_MIN_LON -180.0f #define SAMPLING_MAX_LON 180.0f -static const char declination_table[13][37] = \ +static const int8_t declination_table[13][37] = \ { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ @@ -90,8 +89,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* round down to nearest sampling resolution */ - int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; - int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. @@ -103,7 +102,7 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lat >= SAMPLING_MAX_LAT) { - min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } if (lon <= SAMPLING_MIN_LON) { @@ -111,7 +110,7 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lon >= SAMPLING_MAX_LON) { - min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } /* find index of nearest low sampling point */ @@ -128,7 +127,7 @@ __EXPORT float get_mag_declination(float lat, float lon) float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; - return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; + return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; } float get_lookup_table_val(unsigned lat_index, unsigned lon_index) From 1dfa2f100e37d9798fe50538ed442283dd075aac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 15:33:47 +0200 Subject: [PATCH 49/79] commander: Stop mixing board support and high level code - just accept that non-mandatory leds may or may not be there --- src/modules/commander/commander_helper.cpp | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index fe6c9bfaa5..0fd3c9e9e5 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -199,15 +199,9 @@ int led_init() } /* the blue LED is only available on FMUv1 but not FMUv2 */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - if (ioctl(leds, LED_ON, LED_BLUE)) { - warnx("Blue LED: ioctl fail\n"); - return ERROR; - } - -#endif + (void)ioctl(leds, LED_ON, LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; @@ -217,11 +211,7 @@ int led_init() rgbleds = open(RGBLED_DEVICE_PATH, 0); if (rgbleds == -1) { -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - errx(1, "Unable to open " RGBLED_DEVICE_PATH); -#else - warnx("No RGB LED found"); -#endif + warnx("No RGB LED found at " RGBLED_DEVICE_PATH); } return 0; From 7a4049b12a8be77bb67906c666cddf93286d39fc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 16:55:08 +0200 Subject: [PATCH 50/79] Fix use of declination in estimator, remove bogus measurement value reset on reinit --- src/modules/ekf_att_pos_estimator/estimator.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index a294210459..86b7efafb3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2396,7 +2396,7 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initialHdg = atan2f(-magY, magX); /* true heading is the mag heading minus declination */ - initialHdg -= declination; + initialHdg += declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2528,8 +2528,6 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - magBias.zero(); - magData.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { From 6cb96d074d282af92540b6481d843c2295426773 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 17:01:32 +0200 Subject: [PATCH 51/79] EKF: Introduce proper check flags for sensor init states --- .../fw_att_pos_estimator_main.cpp | 40 ++++++++++++++++--- 1 file changed, 34 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index b87338282a..40729e94b1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -194,6 +194,9 @@ private: bool _baro_init; bool _gps_initialized; uint64_t _gps_start_time; + bool _gyro_valid; + bool _accel_valid; + bool _mag_valid; int _mavlink_fd; @@ -341,6 +344,9 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _baro_init(false), _gps_initialized(false), + _gyro_valid(false), + _accel_valid(false), + _mag_valid(false), _mavlink_fd(-1), _ekf(nullptr) { @@ -617,10 +623,30 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); - /* Reset baro reference if switching to HIL */ + /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + /* system is in HIL now, we got plenty of time - make sure real sensors are off */ + usleep(250000); + +#ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); +#else + /* now read all sensor publications to ensure all real sensor data is purged */ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +#endif + + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + _baro_init = false; _gps_initialized = false; + + /* now skip this loop and get data on the next one */ + continue; } /** @@ -663,12 +689,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; @@ -713,12 +741,14 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + _gyro_valid = true; } if (accel_updated) { _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + _accel_valid = true; } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; @@ -837,6 +867,8 @@ FixedwingEstimator::task_main() if (mag_updated) { + _mag_valid = true; + perf_count(_perf_mag); #ifndef SENSOR_COMBINED_SUB @@ -961,11 +993,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - // Wait long enough to ensure all sensors updated once - // XXX we rather want to check all updated - - - if (hrt_elapsed_time(&_gps_start_time) > 50000) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { // bool home_set; // orb_check(_home_sub, &home_set); From a1e4435e163f4f01885de58dd492d7716863c05e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 30 Apr 2014 17:50:18 +0200 Subject: [PATCH 52/79] esc_calib: corrected name of mc controller --- src/systemcmds/esc_calib/esc_calib.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index ad19966949..7d80af3079 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -182,7 +182,7 @@ esc_calib_main(int argc, char *argv[]) if (orb_updated) { errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" - "\tmultirotor_att_control stop\n" + "\tmc_att_control stop\n" "\tfw_att_control stop\n"); } From a8743184c3d24e6ec8086602241096021d9aba84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 May 2014 16:02:00 +0200 Subject: [PATCH 53/79] Add command to do PWM step inputs --- src/systemcmds/pwm/pwm.c | 106 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 105 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 7c23f38c20..1828c660f8 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -443,6 +443,110 @@ pwm_main(int argc, char *argv[]) exit(0); } } + usleep(2000); + } + exit(0); + + + } else if (!strcmp(argv[1], "steps")) { + + if (set_mask == 0) { + usage("no channels set"); + } + + /* get current servo values */ + struct pwm_output_values last_spos; + + for (unsigned i = 0; i < servo_count; i++) { + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&last_spos.values[i]); + if (ret != OK) + err(1, "PWM_SERVO_GET(%d)", i); + } + + /* perform PWM output */ + + /* Open console directly to grab CTRL-C signal */ + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + + warnx("Running 5 steps. WARNING! Motors will be live in 5 seconds\nPress any key to abort now."); + sleep(5); + + unsigned off = 900; + unsigned idle = 1300; + unsigned full = 2000; + unsigned steps_timings_us[] = {2000, 5000, 20000, 50000}; + + unsigned phase = 0; + unsigned phase_counter = 0; + unsigned const phase_maxcount = 20; + + for ( unsigned steps_timing_index = 0; + steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]); + steps_timing_index++ ) { + + warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]); + + while (1) { + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< 0) { + + int ret = read(0, &c, 1); + if (ret > 0) { + /* reset output to the last value */ + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1< phase_maxcount) { + phase++; + phase_counter = 0; + } + } } exit(0); From 2343aad455aedfb64c22f1ed99820df23c5ac32b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 May 2014 19:16:05 +0200 Subject: [PATCH 54/79] Easystar mixer fix --- ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 2 +- ROMFS/px4fmu_common/mixers/easystar.mix | 84 ++++++++++++++++++++ 2 files changed, 85 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/mixers/easystar.mix diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 465a22c53a..db0e40fc2e 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -5,4 +5,4 @@ sh /etc/init.d/rc.fw_defaults -set MIXER FMU_RET +set MIXER easystar.mix diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.mix new file mode 100644 index 0000000000..610da567f8 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/easystar.mix @@ -0,0 +1,84 @@ +Aileron/rudder/elevator/throttle mixer for PX4FMU +================================================== + +This file defines mixers suitable for controlling a fixed wing aircraft with +aileron, rudder, elevator and throttle controls using PX4FMU. The configuration +assumes the aileron servo(s) are connected to PX4FMU servo output 0, the +elevator to output 1, the rudder to output 2 and the throttle to output 3. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +Aileron mixer +------------- +Two scalers total (output, roll). + +This mixer assumes that the aileron servos are set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +As there is only one output, if using two servos adjustments to compensate for +differences between the servos must be made mechanically. To obtain the correct +motion using a Y cable, the servos can be positioned reversed from one another. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 0 10000 10000 0 -10000 10000 + +Elevator mixer +------------ +Two scalers total (output, roll). + +This mixer assumes that the elevator servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 1 -10000 -10000 0 -10000 10000 + +Rudder mixer +------------ +Two scalers total (output, yaw). + +This mixer assumes that the rudder servo is set up correctly mechanically; +depending on the actual configuration it may be necessary to reverse the scaling +factors (to reverse the servo movement) and adjust the offset, scaling and +endpoints to suit. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 2 -10000 -10000 0 -10000 10000 + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + + +Gimbal / flaps / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 From 1c13b5563b35ab039c30cc7147747222f7936129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 May 2014 19:18:28 +0200 Subject: [PATCH 55/79] Simplify mixer file --- ROMFS/px4fmu_common/mixers/easystar.mix | 59 ++----------------------- 1 file changed, 3 insertions(+), 56 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.mix index 610da567f8..0051ffdbba 100644 --- a/ROMFS/px4fmu_common/mixers/easystar.mix +++ b/ROMFS/px4fmu_common/mixers/easystar.mix @@ -1,26 +1,9 @@ -Aileron/rudder/elevator/throttle mixer for PX4FMU -================================================== - -This file defines mixers suitable for controlling a fixed wing aircraft with -aileron, rudder, elevator and throttle controls using PX4FMU. The configuration -assumes the aileron servo(s) are connected to PX4FMU servo output 0, the -elevator to output 1, the rudder to output 2 and the throttle to output 3. - -Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 -(roll), 1 (pitch) and 3 (thrust). +EASYSTAR / EASYSTAR II MIXER +============================ Aileron mixer ------------- -Two scalers total (output, roll). - -This mixer assumes that the aileron servos are set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. - -As there is only one output, if using two servos adjustments to compensate for -differences between the servos must be made mechanically. To obtain the correct -motion using a Y cable, the servos can be positioned reversed from one another. +One output - would be easy to add support for 2 servos M: 1 O: 10000 10000 0 -10000 10000 @@ -28,12 +11,6 @@ S: 0 0 10000 10000 0 -10000 10000 Elevator mixer ------------ -Two scalers total (output, roll). - -This mixer assumes that the elevator servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. M: 1 O: 10000 10000 0 -10000 10000 @@ -41,12 +18,6 @@ S: 0 1 -10000 -10000 0 -10000 10000 Rudder mixer ------------ -Two scalers total (output, yaw). - -This mixer assumes that the rudder servo is set up correctly mechanically; -depending on the actual configuration it may be necessary to reverse the scaling -factors (to reverse the servo movement) and adjust the offset, scaling and -endpoints to suit. M: 1 O: 10000 10000 0 -10000 10000 @@ -54,31 +25,7 @@ S: 0 2 -10000 -10000 0 -10000 10000 Motor speed mixer ----------------- -Two scalers total (output, thrust). - -This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) -range. Inputs below zero are treated as zero. M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 - - -Gimbal / flaps / payload mixer for last four channels ------------------------------------------------------ - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 From 85ac2796a0f6639b279ab0b7476c98c7a7d551a7 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Thu, 1 May 2014 23:36:35 +0200 Subject: [PATCH 56/79] simplified code, which is now less robust but smaller and easier to read (comment Babushkin) formated source code with fix_code_style.sh (comment Babushkin) fixed Copyright (comment Babushkin) --- src/systemcmds/ver/ver.c | 88 +++++++++++++--------------------------- 1 file changed, 29 insertions(+), 59 deletions(-) diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index c932bc1624..9ae080ee21 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -1,6 +1,6 @@ /**************************************************************************** * -* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +* Copyright (c) 2014 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 @@ -47,7 +47,7 @@ // string constants for version commands static const char sz_ver_hw_str[] = "hw"; -static const char sz_ver_hwcmp_str[]= "hwcmp"; +static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; @@ -62,38 +62,6 @@ static void usage(const char *reason) printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); } -static void ver_githash(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("FW git-hash: "); - } - printf("%s\n", FW_GIT); -} - -static void ver_hwarch(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("HW arch: "); - } - printf("%s\n", HW_ARCH); -} - -static void ver_bdate(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("Build datetime: "); - } - printf("%s %s\n", __DATE__, __TIME__); -} - -static void ver_gcclong(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC toolchain: "); - } - printf("%s\n", __VERSION__); -} - __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) @@ -104,48 +72,50 @@ int ver_main(int argc, char *argv[]) if (argc >= 2) { if (argv[1] != NULL) { if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - ver_hwarch(0); + printf("%s\n", HW_ARCH); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + + } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { // compare 3rd parameter with HW_ARCH string, in case of match, return 0 ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); + if (ret == 0) { - printf("hwver match: %s\n", HW_ARCH); + printf("ver hwcmp match: %s\n", HW_ARCH); } + } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } - } - else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - ver_githash(0); + + } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("%s\n", FW_GIT); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - ver_bdate(0); + + } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + printf("%s %s\n", __DATE__, __TIME__); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - ver_gcclong(0); + + } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("%s\n", __VERSION__); ret = 0; - } - else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { - ver_hwarch(1); - ver_bdate(1); - ver_githash(1); - ver_gcclong(1); + + } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { + printf("HW arch: %s\n", HW_ARCH); + printf("Build datetime: %s %s\n", __DATE__, __TIME__); + printf("FW git-hash: %s\n", FW_GIT); + printf("GCC toolchain: %s\n", __VERSION__); ret = 0; - } - else { + + } else { errx(1, "unknown command.\n"); } - } - else { + + } else { usage("Error, input parameter NULL.\n"); } - } - else { + + } else { usage("Error, not enough parameters."); } From f6d61dfb4cac8e243b92e637d9d11a3a3970d336 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 1 May 2014 23:45:21 +0200 Subject: [PATCH 57/79] mavlink: swap x and y when handling MANUAL_CONTROL mavlink message --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33a4fef126..7c93c1c009 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -432,8 +432,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) memset(&manual, 0, sizeof(manual)); manual.timestamp = hrt_absolute_time(); - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; + manual.pitch = man.x / 1000.0f; + manual.roll = man.y / 1000.0f; manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; From b607933ded6fb2950a278208a018514b4a3c93d4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 May 2014 14:22:20 +0200 Subject: [PATCH 58/79] add .ropeproject to .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 5b345b34a8..c992dbf5a5 100644 --- a/.gitignore +++ b/.gitignore @@ -37,3 +37,4 @@ mavlink/include/mavlink/v0.9/ tags .tags_sorted_by_file .pydevproject +.ropeproject From 047dfc77141e3eb07b9e392e75879ebc0b4bcd6c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 May 2014 13:23:47 +0200 Subject: [PATCH 59/79] romfs pruner: do not try to prune .swp files --- Tools/px_romfs_pruner.py | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index ceef9f9be7..9c88ec3726 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -43,29 +43,30 @@ from __future__ import print_function import argparse import os + def main(): - + # Parse commandline arguments parser = argparse.ArgumentParser(description="ROMFS pruner.") parser.add_argument('--folder', action="store", help="ROMFS scratch folder.") args = parser.parse_args() - + print("Pruning ROMFS files.") - - # go through + + # go through for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file: + if ".zip" in file or ".bin" or ".swp" in file: continue - - file_path = os.path.join(root, file) - + + file_path = os.path.join(root, file) + # read file line by line pruned_content = "" with open(file_path, "r") as f: - for line in f: - + for line in f: + # handle mixer files differently than startup files if file_path.endswith(".mix"): if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): @@ -73,11 +74,11 @@ def main(): else: if not line.isspace() and not line.strip().startswith("#"): pruned_content += line - + # overwrite old scratch file with open(file_path, "w") as f: f.write(pruned_content) - - + + if __name__ == '__main__': - main() \ No newline at end of file + main() From de87d6df69fd797fd3dfb8ac41d2c75e468dffc0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 May 2014 14:23:22 +0200 Subject: [PATCH 60/79] do not copy hidden files to ROMFS --- makefiles/firmware.mk | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 1b646d9e00..69c975acd2 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -113,7 +113,7 @@ endif $(info % GIT_DESC = $(GIT_DESC)) # -# Set a default target so that included makefiles or errors here don't +# Set a default target so that included makefiles or errors here don't # cause confusion. # # XXX We could do something cute here with $(DEFAULT_GOAL) if it's not one @@ -177,7 +177,7 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # # Extra things we should clean # -EXTRA_CLEANS = +EXTRA_CLEANS = # @@ -371,6 +371,8 @@ $(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) $(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) +# delete all files in ROMFS_SCRATCH which start with a . + $(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~ ifneq ($(ROMFS_EXTRA_FILES),) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras $(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras From 3424a65e3229a0bc5a1d78979356436392def264 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 May 2014 14:28:47 +0200 Subject: [PATCH 61/79] update comment in makefile --- makefiles/firmware.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 69c975acd2..60602e76fe 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -371,7 +371,7 @@ $(ROMFS_IMG): $(ROMFS_SCRATCH) $(ROMFS_DEPS) $(GLOBAL_DEPS) $(ROMFS_SCRATCH): $(ROMFS_DEPS) $(GLOBAL_DEPS) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH) $(Q) $(COPYDIR) $(ROMFS_ROOT)/* $(ROMFS_SCRATCH) -# delete all files in ROMFS_SCRATCH which start with a . +# delete all files in ROMFS_SCRATCH which start with a . or end with a ~ $(Q) $(RM) $(ROMFS_SCRATCH)/*/.[!.]* $(ROMFS_SCRATCH)/*/*~ ifneq ($(ROMFS_EXTRA_FILES),) $(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras From d5e463352ddf501fa9cfcf7921fd31bae1153ce1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 2 May 2014 17:34:50 +0200 Subject: [PATCH 62/79] romfs pruner: fix filename check --- Tools/px_romfs_pruner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 9c88ec3726..fcc40b09e1 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" or ".swp" in file: + if ".zip" in file or ".bin" in file or ".swp" in file: continue file_path = os.path.join(root, file) From 2de01964e2ca30344f64df69500775a4eb7af36d Mon Sep 17 00:00:00 2001 From: Kynos Date: Fri, 2 May 2014 22:00:34 +0200 Subject: [PATCH 63/79] Reset MS5611 baro sensor after an error Reset MS5611 baro sensor after an error in order to avoid endless error loops --- src/drivers/ms5611/ms5611.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0ef056273f..3fe1b0abcb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -526,6 +526,7 @@ void MS5611::cycle() { int ret; + unsigned dummy; /* collection phase? */ if (_collect_phase) { @@ -542,6 +543,8 @@ MS5611::cycle() } else { //log("collection error %d", ret); } + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -573,6 +576,8 @@ MS5611::cycle() ret = measure(); if (ret != OK) { //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; From 22f262a241256e54aa6a03763689bdcdeb39be87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 May 2014 12:40:11 +0200 Subject: [PATCH 64/79] host tests: Fix missing newlines --- Tools/tests-host/autodeclination_test.cpp | 2 +- Tools/tests-host/mixer_test.cpp | 2 +- Tools/tests-host/sbus2_test.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp index 6c751dc1e7..93bc340bb3 100644 --- a/Tools/tests-host/autodeclination_test.cpp +++ b/Tools/tests-host/autodeclination_test.cpp @@ -25,4 +25,4 @@ int main(int argc, char *argv[]) { printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination); -} \ No newline at end of file +} diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index e311617f96..06499afd0e 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -11,4 +11,4 @@ int main(int argc, char *argv[]) { test_mixer(3, args); test_conv(1, args); -} \ No newline at end of file +} diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index 281903cf68..d8fcb695d8 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -72,4 +72,4 @@ int main(int argc, char *argv[]) { warnx("Test aborted, errno: %d", ret); } -} \ No newline at end of file +} From ee580206b4fd3e79f27db42717987dd414a2a215 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 May 2014 14:06:38 +0200 Subject: [PATCH 65/79] mavlink: Only sending HIL control commands if the system is actually armed --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce16455..b538aa28ba 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -819,11 +819,11 @@ protected: void send(const hrt_abstime t) { - bool updated = status_sub->update(t); - updated |= pos_sp_triplet_sub->update(t); - updated |= act_sub->update(t); + bool updated = act_sub->update(t); + (void)pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); - if (updated) { + if (updated && (status.arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; From 0e31b5935ebefef3b95d7e39dbf5b1435a4942a1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 May 2014 15:01:07 +0200 Subject: [PATCH 66/79] remove trailing whitespace --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b538aa28ba..ab733e4425 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1353,7 +1353,7 @@ protected: uint8_t orientation = 0; uint8_t covariance = 20; - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); } }; From 5f786af8fa428c8ab684251310de6c3fbe795e8f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 4 May 2014 15:02:00 +0200 Subject: [PATCH 67/79] mavlink: status is a pointer --- src/modules/mavlink/mavlink_messages.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index ab733e4425..bef8a5a55e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -823,7 +823,7 @@ protected: (void)pos_sp_triplet_sub->update(t); (void)status_sub->update(t); - if (updated && (status.arming_state == ARMING_STATE_ARMED)) { + if (updated && (status->arming_state == ARMING_STATE_ARMED)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; From f2094b9a1f9ea492d0a3c4c294a756332e83404d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 4 May 2014 16:05:15 +0200 Subject: [PATCH 68/79] Unused deprecated modules removed: att_pos_estimator_ekf, fixedwing_att_control, fixedwing_pos_control, position_estimator, position_estimator_mc, sdlog --- .../att_pos_estimator_ekf/KalmanNav.cpp | 812 ----------------- .../att_pos_estimator_ekf/KalmanNav.hpp | 194 ---- .../att_pos_estimator_ekf/kalman_main.cpp | 157 ---- src/modules/att_pos_estimator_ekf/module.mk | 42 - src/modules/att_pos_estimator_ekf/params.c | 49 - .../fixedwing_att_control_att.c | 169 ---- .../fixedwing_att_control_att.h | 51 -- .../fixedwing_att_control_main.c | 367 -------- .../fixedwing_att_control_rate.c | 211 ----- .../fixedwing_att_control_rate.h | 48 - src/modules/fixedwing_att_control/module.mk | 42 - .../fixedwing_pos_control_main.c | 479 ---------- src/modules/fixedwing_pos_control/module.mk | 40 - src/modules/position_estimator/module.mk | 44 - .../position_estimator_main.c | 423 --------- .../codegen/kalman_dlqe1.c | 58 -- .../codegen/kalman_dlqe1.h | 30 - .../codegen/kalman_dlqe1_initialize.c | 31 - .../codegen/kalman_dlqe1_initialize.h | 30 - .../codegen/kalman_dlqe1_terminate.c | 31 - .../codegen/kalman_dlqe1_terminate.h | 30 - .../codegen/kalman_dlqe1_types.h | 16 - .../codegen/kalman_dlqe2.c | 119 --- .../codegen/kalman_dlqe2.h | 32 - .../codegen/kalman_dlqe2_initialize.c | 31 - .../codegen/kalman_dlqe2_initialize.h | 32 - .../codegen/kalman_dlqe2_terminate.c | 31 - .../codegen/kalman_dlqe2_terminate.h | 32 - .../codegen/kalman_dlqe2_types.h | 16 - .../codegen/kalman_dlqe3.c | 137 --- .../codegen/kalman_dlqe3.h | 33 - .../codegen/kalman_dlqe3_data.c | 32 - .../codegen/kalman_dlqe3_data.h | 38 - .../codegen/kalman_dlqe3_initialize.c | 47 - .../codegen/kalman_dlqe3_initialize.h | 33 - .../codegen/kalman_dlqe3_terminate.c | 31 - .../codegen/kalman_dlqe3_terminate.h | 33 - .../codegen/kalman_dlqe3_types.h | 16 - .../codegen/positionKalmanFilter1D.c | 136 --- .../codegen/positionKalmanFilter1D.h | 31 - .../codegen/positionKalmanFilter1D_dT.c | 157 ---- .../codegen/positionKalmanFilter1D_dT.h | 31 - .../positionKalmanFilter1D_dT_initialize.c | 31 - .../positionKalmanFilter1D_dT_initialize.h | 31 - .../positionKalmanFilter1D_dT_terminate.c | 31 - .../positionKalmanFilter1D_dT_terminate.h | 31 - .../codegen/positionKalmanFilter1D_dT_types.h | 16 - .../positionKalmanFilter1D_initialize.c | 31 - .../positionKalmanFilter1D_initialize.h | 31 - .../positionKalmanFilter1D_terminate.c | 31 - .../positionKalmanFilter1D_terminate.h | 31 - .../codegen/positionKalmanFilter1D_types.h | 16 - .../position_estimator_mc/codegen/randn.c | 524 ----------- .../position_estimator_mc/codegen/randn.h | 33 - .../position_estimator_mc/codegen/rtGetInf.c | 139 --- .../position_estimator_mc/codegen/rtGetInf.h | 23 - .../position_estimator_mc/codegen/rtGetNaN.c | 96 -- .../position_estimator_mc/codegen/rtGetNaN.h | 21 - .../codegen/rt_nonfinite.c | 87 -- .../codegen/rt_nonfinite.h | 53 -- .../position_estimator_mc/codegen/rtwtypes.h | 159 ---- .../position_estimator_mc/kalman_dlqe1.m | 3 - .../position_estimator_mc/kalman_dlqe2.m | 9 - .../position_estimator_mc/kalman_dlqe3.m | 17 - src/modules/position_estimator_mc/module.mk | 60 -- .../positionKalmanFilter1D.m | 19 - .../positionKalmanFilter1D_dT.m | 26 - .../position_estimator_mc_main.c | 515 ----------- .../position_estimator_mc_params.c | 68 -- .../position_estimator_mc_params.h | 69 -- src/modules/sdlog/module.mk | 43 - src/modules/sdlog/sdlog.c | 840 ------------------ src/modules/sdlog/sdlog_ringbuffer.c | 91 -- src/modules/sdlog/sdlog_ringbuffer.h | 91 -- 74 files changed, 7668 deletions(-) delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp delete mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/module.mk delete mode 100644 src/modules/att_pos_estimator_ekf/params.c delete mode 100644 src/modules/fixedwing_att_control/fixedwing_att_control_att.c delete mode 100644 src/modules/fixedwing_att_control/fixedwing_att_control_att.h delete mode 100644 src/modules/fixedwing_att_control/fixedwing_att_control_main.c delete mode 100644 src/modules/fixedwing_att_control/fixedwing_att_control_rate.c delete mode 100644 src/modules/fixedwing_att_control/fixedwing_att_control_rate.h delete mode 100644 src/modules/fixedwing_att_control/module.mk delete mode 100644 src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c delete mode 100644 src/modules/fixedwing_pos_control/module.mk delete mode 100644 src/modules/position_estimator/module.mk delete mode 100644 src/modules/position_estimator/position_estimator_main.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h delete mode 100755 src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h delete mode 100755 src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h delete mode 100755 src/modules/position_estimator_mc/codegen/randn.c delete mode 100755 src/modules/position_estimator_mc/codegen/randn.h delete mode 100755 src/modules/position_estimator_mc/codegen/rtGetInf.c delete mode 100755 src/modules/position_estimator_mc/codegen/rtGetInf.h delete mode 100755 src/modules/position_estimator_mc/codegen/rtGetNaN.c delete mode 100755 src/modules/position_estimator_mc/codegen/rtGetNaN.h delete mode 100755 src/modules/position_estimator_mc/codegen/rt_nonfinite.c delete mode 100755 src/modules/position_estimator_mc/codegen/rt_nonfinite.h delete mode 100755 src/modules/position_estimator_mc/codegen/rtwtypes.h delete mode 100755 src/modules/position_estimator_mc/kalman_dlqe1.m delete mode 100755 src/modules/position_estimator_mc/kalman_dlqe2.m delete mode 100755 src/modules/position_estimator_mc/kalman_dlqe3.m delete mode 100644 src/modules/position_estimator_mc/module.mk delete mode 100755 src/modules/position_estimator_mc/positionKalmanFilter1D.m delete mode 100755 src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m delete mode 100755 src/modules/position_estimator_mc/position_estimator_mc_main.c delete mode 100755 src/modules/position_estimator_mc/position_estimator_mc_params.c delete mode 100755 src/modules/position_estimator_mc/position_estimator_mc_params.h delete mode 100644 src/modules/sdlog/module.mk delete mode 100644 src/modules/sdlog/sdlog.c delete mode 100644 src/modules/sdlog/sdlog_ringbuffer.c delete mode 100644 src/modules/sdlog/sdlog_ringbuffer.h diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 5cf84542b1..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,812 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include - -#include "KalmanNav.hpp" -#include -#include - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - memset(&ref, 0, sizeof(ref)); - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - map_projection_init(&ref, lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(&ref, lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = lat * M_RAD_TO_DEG; - _localPos.ref_lon = lon * M_RAD_TO_DEG; - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index 24df153cb4..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,194 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription _sensors; /**< sensors sub. */ - uORB::Subscription _gps; /**< gps sub. */ - uORB::Subscription _param_update; /**< parameter update sub. */ - // publications - uORB::Publication _pos; /**< position pub. */ - uORB::Publication _localPos; /**< local position pub. */ - uORB::Publication _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - struct map_projection_reference_s ref; /**< local projection reference */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d6..0000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p ]"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d950..0000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edeade..0000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c deleted file mode 100644 index 2aeca3a98e..0000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file fixedwing_att_control_rate.c - * Implementation of a fixed wing attitude controller. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "fixedwing_att_control_att.h" - - -struct fw_att_control_params { - float roll_p; - float rollrate_lim; - float pitch_p; - float pitchrate_lim; - float yawrate_lim; - float pitch_roll_compensation_p; -}; - -struct fw_pos_control_param_handles { - param_t roll_p; - param_t rollrate_lim; - param_t pitch_p; - param_t pitchrate_lim; - param_t yawrate_lim; - param_t pitch_roll_compensation_p; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p); - -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->roll_p = param_find("FW_ROLL_P"); - h->rollrate_lim = param_find("FW_ROLLR_LIM"); - h->pitch_p = param_find("FW_PITCH_P"); - h->pitchrate_lim = param_find("FW_PITCHR_LIM"); - h->yawrate_lim = param_find("FW_YAWR_LIM"); - h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p) -{ - param_get(h->roll_p, &(p->roll_p)); - param_get(h->rollrate_lim, &(p->rollrate_lim)); - param_get(h->pitch_p, &(p->pitch_p)); - param_get(h->pitchrate_lim, &(p->pitchrate_lim)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); - param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p)); - - return OK; -} - -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_att_control_params p; - static struct fw_pos_control_param_handles h; - - static PID_t roll_controller; - static PID_t pitch_controller; - - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); - } - - /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); - - - /* Pitch (P) */ - - /* compensate feedforward for loss of lift due to non-horizontal angle of wing */ - float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body)); - /* set pitch plus feedforward roll compensation */ - rates_sp->pitch = pid_calculate(&pitch_controller, - att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); - - /* Yaw (from coordinated turn constraint or lateral force) */ - rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) - / (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch)); - -// printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h deleted file mode 100644 index 600e35b898..0000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h +++ /dev/null @@ -1,51 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file Fixed Wing Attitude Control */ - -#ifndef FIXEDWING_ATT_CONTROL_ATT_H_ -#define FIXEDWING_ATT_CONTROL_ATT_H_ - -#include -#include -#include -#include - -int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, - const float speed_body[], - struct vehicle_rates_setpoint_s *rates_sp); - -#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c deleted file mode 100644 index b6b4546c23..0000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ /dev/null @@ -1,367 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Doug Weibel - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file fixedwing_att_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "fixedwing_att_control_rate.h" -#include "fixedwing_att_control_att.h" - -/* Prototypes */ -/** - * Deamon management function. - */ -__EXPORT int fixedwing_att_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_att_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -/* Main Thread */ -int fixedwing_att_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing att control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct manual_control_setpoint_s manual_sp; - memset(&manual_sp, 0, sizeof(manual_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - - /* output structs */ - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* subscribe */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* Setup of loop */ - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float speed_body[3] = {0.0f, 0.0f, 0.0f}; - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - poll(&fds, 1, 500); - - /* Check if there is a new position measurement or attitude setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool att_sp_updated; - orb_check(att_sp_sub, &att_sp_updated); - - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (att_sp_updated) - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - - if (att.R_valid) { - speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz; - speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz; - speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz; - - } else { - speed_body[0] = 0; - speed_body[1] = 0; - speed_body[2] = 0; - - printf("FW ATT CONTROL: Did not get a valid R\n"); - } - } - - orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - /* set manual setpoints if required */ - if (control_mode.flag_control_manual_enabled) { - if (control_mode.flag_control_attitude_enabled) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } - - /* execute attitude control if requested */ - if (control_mode.flag_control_attitude_enabled) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } - - /* publish rates */ - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - - /* sanity check and publish actuator outputs */ - if (isfinite(actuators.control[0]) && - isfinite(actuators.control[1]) && - isfinite(actuators.control[2]) && - isfinite(actuators.control[3])) { - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - - printf("[fixedwing_att_control] exiting, stopping all motors.\n"); - thread_running = false; - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - - - close(att_sub); - close(actuator_pub); - close(rates_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_att_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_att_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_att_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_att_control is running\n"); - - } else { - printf("\tfixedwing_att_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c deleted file mode 100644 index cdab39edcf..0000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ /dev/null @@ -1,211 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file fixedwing_att_control_rate.c - * @author Thomas Gubler - * - * Implementation of a fixed wing attitude controller. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "fixedwing_att_control_rate.h" - -/* - * Controller parameters, accessible via MAVLink - * - */ -// Roll control parameters -PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f); -PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f); // Roll rate limit in radians/sec, applies to the roll controller -PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f); -PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f); - -//Pitch control parameters -PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f); -PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f); -PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f); // Pitch rate limit in radians/sec, applies to the pitch controller -PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f); - -//Yaw control parameters //XXX TODO this is copy paste, asign correct values -PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f); -PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f); -PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f); // Yaw rate limit in radians/sec - -/* feedforward compensation */ -PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f); /**< throttle to pitch coupling feedforward */ - -struct fw_rate_control_params { - float rollrate_p; - float rollrate_i; - float rollrate_awu; - float pitchrate_p; - float pitchrate_i; - float pitchrate_awu; - float yawrate_p; - float yawrate_i; - float yawrate_awu; - float pitch_thr_ff; -}; - -struct fw_rate_control_param_handles { - param_t rollrate_p; - param_t rollrate_i; - param_t rollrate_awu; - param_t pitchrate_p; - param_t pitchrate_i; - param_t pitchrate_awu; - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_awu; - param_t pitch_thr_ff; -}; - - - -/* Internal Prototypes */ -static int parameters_init(struct fw_rate_control_param_handles *h); -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p); - -static int parameters_init(struct fw_rate_control_param_handles *h) -{ - /* PID parameters */ - h->rollrate_p = param_find("FW_ROLLR_P"); //TODO define rate params for fixed wing - h->rollrate_i = param_find("FW_ROLLR_I"); - h->rollrate_awu = param_find("FW_ROLLR_AWU"); - - h->pitchrate_p = param_find("FW_PITCHR_P"); - h->pitchrate_i = param_find("FW_PITCHR_I"); - h->pitchrate_awu = param_find("FW_PITCHR_AWU"); - - h->yawrate_p = param_find("FW_YAWR_P"); - h->yawrate_i = param_find("FW_YAWR_I"); - h->yawrate_awu = param_find("FW_YAWR_AWU"); - h->pitch_thr_ff = param_find("FW_PITCH_THR_P"); - - return OK; -} - -static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p) -{ - param_get(h->rollrate_p, &(p->rollrate_p)); - param_get(h->rollrate_i, &(p->rollrate_i)); - param_get(h->rollrate_awu, &(p->rollrate_awu)); - param_get(h->pitchrate_p, &(p->pitchrate_p)); - param_get(h->pitchrate_i, &(p->pitchrate_i)); - param_get(h->pitchrate_awu, &(p->pitchrate_awu)); - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->pitch_thr_ff, &(p->pitch_thr_ff)); - - return OK; -} - -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators) -{ - static int counter = 0; - static bool initialized = false; - - static struct fw_rate_control_params p; - static struct fw_rate_control_param_handles h; - - static PID_t roll_rate_controller; - static PID_t pitch_rate_controller; - static PID_t yaw_rate_controller; - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (!initialized) { - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - initialized = true; - } - - /* load new parameters with lower rate */ - if (counter % 100 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); - } - - - /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - counter++; - - return 0; -} - - - diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h deleted file mode 100644 index 500e3e1971..0000000000 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h +++ /dev/null @@ -1,48 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* @file Fixed Wing Attitude Rate Control */ - -#ifndef FIXEDWING_ATT_CONTROL_RATE_H_ -#define FIXEDWING_ATT_CONTROL_RATE_H_ - -#include -#include - -int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], - struct actuator_controls_s *actuators); - -#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */ diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/fixedwing_att_control/module.mk deleted file mode 100644 index fd1a8724a5..0000000000 --- a/src/modules/fixedwing_att_control/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Fixedwing Attitude Control application -# - -MODULE_COMMAND = fixedwing_att_control - -SRCS = fixedwing_att_control_main.c \ - fixedwing_att_control_att.c \ - fixedwing_att_control_rate.c diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c deleted file mode 100644 index 888dd09423..0000000000 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ /dev/null @@ -1,479 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Doug Weibel - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file fixedwing_pos_control.c - * Implementation of a fixed wing attitude controller. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* - * Controller parameters, accessible via MAVLink - * - */ -PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f); -PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value -PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track -PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians -PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f); -PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); /**< Pitch angle limit in radians per second */ - -struct fw_pos_control_params { - float heading_p; - float headingr_p; - float headingr_i; - float headingr_lim; - float xtrack_p; - float altitude_p; - float roll_lim; - float pitch_lim; -}; - -struct fw_pos_control_param_handles { - param_t heading_p; - param_t headingr_p; - param_t headingr_i; - param_t headingr_lim; - param_t xtrack_p; - param_t altitude_p; - param_t roll_lim; - param_t pitch_lim; -}; - - -struct planned_path_segments_s { - bool segment_type; - double start_lat; // Start of line or center of arc - double start_lon; - double end_lat; - double end_lon; - float radius; // Radius of arc - float arc_start_bearing; // Bearing from center to start of arc - float arc_sweep; // Angle (radians) swept out by arc around center. - // Positive for clockwise, negative for counter-clockwise -}; - - -/* Prototypes */ -/* Internal Prototypes */ -static int parameters_init(struct fw_pos_control_param_handles *h); -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p); - -/** - * Deamon management function. - */ -__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Variables */ -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - - -/** - * Parameter management - */ -static int parameters_init(struct fw_pos_control_param_handles *h) -{ - /* PID parameters */ - h->heading_p = param_find("FW_HEAD_P"); - h->headingr_p = param_find("FW_HEADR_P"); - h->headingr_i = param_find("FW_HEADR_I"); - h->headingr_lim = param_find("FW_HEADR_LIM"); - h->xtrack_p = param_find("FW_XTRACK_P"); - h->altitude_p = param_find("FW_ALT_P"); - h->roll_lim = param_find("FW_ROLL_LIM"); - h->pitch_lim = param_find("FW_PITCH_LIM"); - - return OK; -} - -static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) -{ - param_get(h->heading_p, &(p->heading_p)); - param_get(h->headingr_p, &(p->headingr_p)); - param_get(h->headingr_i, &(p->headingr_i)); - param_get(h->headingr_lim, &(p->headingr_lim)); - param_get(h->xtrack_p, &(p->xtrack_p)); - param_get(h->altitude_p, &(p->altitude_p)); - param_get(h->roll_lim, &(p->roll_lim)); - param_get(h->pitch_lim, &(p->pitch_lim)); - - return OK; -} - - -/* Main Thread */ -int fixedwing_pos_control_thread_main(int argc, char *argv[]) -{ - /* read arguments */ - bool verbose = false; - - for (int i = 1; i < argc; i++) { - if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { - verbose = true; - } - } - - /* welcome user */ - printf("[fixedwing pos control] started\n"); - - /* declare and safely initialize all structs */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - struct vehicle_global_position_s start_pos; // Temporary variable, replace with - memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available - struct vehicle_global_position_setpoint_s global_setpoint; - memset(&global_setpoint, 0, sizeof(global_setpoint)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct crosstrack_error_s xtrack_err; - memset(&xtrack_err, 0, sizeof(xtrack_err)); - struct parameter_update_s param_update; - memset(¶m_update, 0, sizeof(param_update)); - - /* output structs */ - struct vehicle_attitude_setpoint_s attitude_setpoint; - memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); - - /* publish attitude setpoint */ - attitude_setpoint.roll_body = 0.0f; - attitude_setpoint.pitch_body = 0.0f; - attitude_setpoint.yaw_body = 0.0f; - attitude_setpoint.thrust = 0.0f; - orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); - - /* subscribe */ - int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - - /* Setup of loop */ - struct pollfd fds[2] = { - { .fd = param_sub, .events = POLLIN }, - { .fd = att_sub, .events = POLLIN } - }; - bool global_sp_updated_set_once = false; - - float psi_track = 0.0f; - - int counter = 0; - - struct fw_pos_control_params p; - struct fw_pos_control_param_handles h; - - PID_t heading_controller; - PID_t heading_rate_controller; - PID_t offtrack_controller; - PID_t altitude_controller; - - parameters_init(&h); - parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value - - /* error and performance monitoring */ - perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); - perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err"); - - while (!thread_should_exit) { - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(fw_err_perf); - - } else if (ret == 0) { - /* no return value, ignore */ - } else { - - /* only update parameters if they changed */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); - - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value - } - - /* only run controller if attitude changed */ - if (fds[1].revents & POLLIN) { - - - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - /* check if there is a new position or setpoint */ - bool pos_updated; - orb_check(global_pos_sub, &pos_updated); - bool global_sp_updated; - orb_check(global_setpoint_sub, &global_sp_updated); - - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - - if (pos_updated) { - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); - } - - if (global_sp_updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); - start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) - global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - printf("next wp direction: %0.4f\n", (double)psi_track); - } - - /* Simple Horizontal Control */ - if (global_sp_updated_set_once) { - // if (counter % 100 == 0) - // printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon); - - /* calculate crosstrack error */ - // Only the case of a straight line track following handled so far - int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, - (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, - (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); - - // XXX what is xtrack_err.past_end? - if (distance_res == OK /*&& !xtrack_err.past_end*/) { - - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance - - float psi_c = psi_track + delta_psi_c; - float psi_e = psi_c - att.yaw; - - /* wrap difference back onto -pi..pi range */ - psi_e = _wrap_pi(psi_e); - - if (verbose) { - printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance); - printf("delta_psi_c %.4f ", (double)delta_psi_c); - printf("psi_c %.4f ", (double)psi_c); - printf("att.yaw %.4f ", (double)att.yaw); - printf("psi_e %.4f ", (double)psi_e); - } - - /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); - float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following - float psi_rate_c = delta_psi_rate_c + psi_rate_track; - - /* limit turn rate */ - if (psi_rate_c > p.headingr_lim) { - psi_rate_c = p.headingr_lim; - - } else if (psi_rate_c < -p.headingr_lim) { - psi_rate_c = -p.headingr_lim; - } - - float psi_rate_e = psi_rate_c - att.yawspeed; - - // XXX sanity check: Assume 10 m/s stall speed and no stall condition - float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - - if (ground_speed < 10.0f) { - ground_speed = 10.0f; - } - - float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); - - if (verbose) { - printf("psi_rate_c %.4f ", (double)psi_rate_c); - printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled); - printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body); - } - - if (verbose && counter % 100 == 0) - printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c); - - } else { - if (verbose && counter % 100 == 0) - printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end); - } - - /* Very simple Altitude Control */ - if (pos_updated) { - - //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); - - } - - // XXX need speed control - attitude_setpoint.thrust = 0.7f; - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint); - - /* measure in what intervals the controller runs */ - perf_count(fw_interval_perf); - - counter++; - - } else { - // XXX no setpoint, decent default needed (loiter?) - } - } - } - } - - printf("[fixedwing_pos_control] exiting.\n"); - thread_running = false; - - - close(attitude_setpoint_pub); - - fflush(stdout); - exit(0); - - return 0; - -} - -/* Startup Functions */ - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("fixedwing_pos_control already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("fixedwing_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 2048, - fixedwing_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tfixedwing_pos_control is running\n"); - - } else { - printf("\tfixedwing_pos_control not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/fixedwing_pos_control/module.mk b/src/modules/fixedwing_pos_control/module.mk deleted file mode 100644 index b976377e93..0000000000 --- a/src/modules/fixedwing_pos_control/module.mk +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Fixedwing PositionControl application -# - -MODULE_COMMAND = fixedwing_pos_control - -SRCS = fixedwing_pos_control_main.c diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk deleted file mode 100644 index f64095d9d1..0000000000 --- a/src/modules/position_estimator/module.mk +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator - -# XXX this should be converted to a deamon, its a pretty bad example app -MODULE_PRIORITY = SCHED_PRIORITY_DEFAULT -MODULE_STACKSIZE = 4096 - -SRCS = position_estimator_main.c diff --git a/src/modules/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c deleted file mode 100644 index e849452993..0000000000 --- a/src/modules/position_estimator/position_estimator_main.c +++ /dev/null @@ -1,423 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Thomas Gubler - * Julian Oes - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file position_estimator_main.c - * Model-identification based position estimator for multirotors - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define N_STATES 6 -#define ERROR_COVARIANCE_INIT 3 -#define R_EARTH 6371000.0 - -#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 -#define REPROJECTION_COUNTER_LIMIT 125 - -__EXPORT int position_estimator_main(int argc, char *argv[]); - -static uint16_t position_estimator_counter_position_information; - -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; - -} - -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_project(double lat, double lon, float *x, float *y) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); - - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); -} - -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -static void map_projection_reproject(float x, float y, double *lat, double *lon) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); - double sin_c = sin(c); - double cos_c = cos(c); - - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); - - } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } - } - -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - -} - -/**************************************************************************** - * main - ****************************************************************************/ - -int position_estimator_main(int argc, char *argv[]) -{ - - /* welcome user */ - printf("[multirotor position_estimator] started\n"); - - /* initialize values */ - static float u[2] = {0, 0}; - static float z[3] = {0, 0, 0}; - static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0}; - static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0, - ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0 - }; - - static float xapo1[N_STATES]; - static float Papo1[36]; - - static float gps_covariance[3] = {0.0f, 0.0f, 0.0f}; - - static uint16_t counter = 0; - position_estimator_counter_position_information = 0; - - uint8_t predict_only = 1; - - bool gps_valid = false; - - bool new_initialization = true; - - static double lat_current = 0.0d;//[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - float alt_current = 0.0f; - - - //TODO: handle flight without gps but with estimator - - /* subscribe to vehicle status, attitude, gps */ - struct vehicle_gps_position_s gps; - gps.fix_type = 0; - struct vehicle_status_s vstatus; - struct vehicle_attitude_s att; - - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* subscribe to attitude at 100 Hz */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - - /* wait until gps signal turns valid, only then can we initialize the projection */ - while (gps.fix_type < 3) { - struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds, 1, 5000)) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - /* Read wether the vehicle status changed */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - gps_valid = (gps.fix_type > 2); - } - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - - /* publish global position messages only after first GPS message */ - struct vehicle_local_position_s local_pos = { - .x = 0, - .y = 0, - .z = 0 - }; - orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - - printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - while (1) { - - /*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */ - struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} }; - - if (poll(fds, 1, 5000) <= 0) { - /* error / timeout */ - } else { - - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - /* got attitude, updating pos as well */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); - - /*copy attitude */ - u[0] = att.roll; - u[1] = att.pitch; - - /* initialize map projection with the last estimate (not at full rate) */ - if (gps.fix_type > 2) { - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1])); - - local_pos.x = z[0]; - local_pos.y = z[1]; - /* negative offset from initialization altitude */ - local_pos.z = alt_current - (gps.alt) * 1e-3; - - - orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); - } - - - // gps_covariance[0] = gps.eph; //TODO: needs scaling - // gps_covariance[1] = gps.eph; - // gps_covariance[2] = gps.epv; - - // } else { - // /* we can not use the gps signal (it is of low quality) */ - // predict_only = 1; - // } - - // // predict_only = 0; //TODO: only for testing, removeme, XXX - // // z[0] = sinf(((float)counter)/180.0f*3.14159265f); //TODO: only for testing, removeme, XXX - // // usleep(100000); //TODO: only for testing, removeme, XXX - - - // /*Get new estimation (this is calculated in the plane) */ - // //TODO: if new_initialization == true: use 0,0,0, else use xapo - // if (true == new_initialization) { //TODO,XXX: uncomment! - // xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane - // xapo[2] = 0; - // xapo[4] = 0; - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - - // } else { - // position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1); - // } - - - - // /* Copy values from xapo1 to xapo */ - // int i; - - // for (i = 0; i < N_STATES; i++) { - // xapo[i] = xapo1[i]; - // } - - // if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) { - // /* Reproject from plane to geographic coordinate system */ - // // map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment! - // map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme - // // //DEBUG - // // if(counter%500 == 0) - // // { - // // printf("phi_1: %.10f\n", phi_1); - // // printf("lambda_0: %.10f\n", lambda_0); - // // printf("lat_estimated: %.10f\n", lat_current); - // // printf("lon_estimated: %.10f\n", lon_current); - // // printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]); - // // fflush(stdout); - // // - // // } - - // // if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5])) - // // { - // /* send out */ - - // global_pos.lat = lat_current; - // global_pos.lon = lon_current; - // global_pos.alt = xapo1[4]; - // global_pos.vx = xapo1[1]; - // global_pos.vy = xapo1[3]; - // global_pos.vz = xapo1[5]; - - /* publish current estimate */ - // orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos); - // } - // else - // { - // printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]); - // fflush(stdout); - // } - - // } - - counter++; - } - - } - - return 0; -} - - diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c deleted file mode 100755 index 977565b8ed..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c +++ /dev/null @@ -1,58 +0,0 @@ -/* - * kalman_dlqe1.c - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], - const real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z); - printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2])); - real32_T y; - int32_T i0; - real32_T b_y[3]; - int32_T i1; - real32_T f0; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - b_y[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_y[i0] += C[i1] * A[i1 + 3 * i0]; - } - - y += b_y[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + K[i0] * y; - } - //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2])); -} - -/* End of code generation (kalman_dlqe1.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h deleted file mode 100755 index 2f53305633..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_H__ -#define __KALMAN_DLQE1_H__ -/* Include files */ -#include -#include - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe1.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c deleted file mode 100755 index 6627f76e9c..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_initialize.c - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe1_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h deleted file mode 100755 index a77eb57123..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_initialize.h - * - * Code generation for function 'kalman_dlqe1_initialize' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_INITIALIZE_H__ -#define __KALMAN_DLQE1_INITIALIZE_H__ -/* Include files */ -#include -#include - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_initialize(void); -#endif -/* End of code generation (kalman_dlqe1_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c deleted file mode 100755 index a65536f792..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe1_terminate.c - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe1.h" -#include "kalman_dlqe1_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe1_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe1_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h deleted file mode 100755 index 100c7f76cf..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h +++ /dev/null @@ -1,30 +0,0 @@ -/* - * kalman_dlqe1_terminate.h - * - * Code generation for function 'kalman_dlqe1_terminate' - * - * C source code generated on: Wed Feb 13 20:34:32 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TERMINATE_H__ -#define __KALMAN_DLQE1_TERMINATE_H__ -/* Include files */ -#include -#include - -#include "rtwtypes.h" -#include "kalman_dlqe1_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe1_terminate(void); -#endif -/* End of code generation (kalman_dlqe1_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h deleted file mode 100755 index d4b2c2d61d..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe1_types.h - * - * Code generation for function 'kalman_dlqe1' - * - * C source code generated on: Wed Feb 13 20:34:31 2013 - * - */ - -#ifndef __KALMAN_DLQE1_TYPES_H__ -#define __KALMAN_DLQE1_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe1_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c deleted file mode 100755 index 11b9990642..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c +++ /dev/null @@ -1,119 +0,0 @@ -/* - * kalman_dlqe2.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f1; - real32_T f2; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f1 = (real32_T)fabs(u0); - f2 = (real32_T)fabs(u1); - if (rtIsInfF(u1)) { - if (f1 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f1 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f2 == 0.0F) { - y = 1.0F; - } else if (f2 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)pow(u0, u1); - } - } - - return y; -} - -void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const - real32_T x_aposteriori_k[3], real32_T z, real32_T - x_aposteriori[3]) -{ - //printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3)); - //printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3)); - real32_T A[9]; - real32_T y; - int32_T i0; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T b_k1[3]; - int32_T i1; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T f0; - A[0] = 1.0F; - A[3] = dt; - A[6] = 0.5F * rt_powf_snf(dt, 2.0F); - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = dt; - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - A[2 + 3 * i0] = (real32_T)iv0[i0]; - b_k1[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; - } - - y += b_k1[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - b_k1[0] = k1; - b_k1[1] = k2; - b_k1[2] = k3; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + b_k1[i0] * y; - } -} - -/* End of code generation (kalman_dlqe2.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h deleted file mode 100755 index 30170ae22b..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __KALMAN_DLQE2_H__ -#define __KALMAN_DLQE2_H__ -/* Include files */ -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe2.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c deleted file mode 100755 index de5a1d8aa8..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_initialize.c - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (kalman_dlqe2_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h deleted file mode 100755 index 3d507ff318..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_initialize.h - * - * Code generation for function 'kalman_dlqe2_initialize' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_INITIALIZE_H__ -#define __KALMAN_DLQE2_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_initialize(void); -#endif -/* End of code generation (kalman_dlqe2_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c deleted file mode 100755 index 0757c878cb..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe2_terminate.c - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe2.h" -#include "kalman_dlqe2_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe2_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe2_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h deleted file mode 100755 index 23995020b4..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe2_terminate.h - * - * Code generation for function 'kalman_dlqe2_terminate' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TERMINATE_H__ -#define __KALMAN_DLQE2_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe2_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe2_terminate(void); -#endif -/* End of code generation (kalman_dlqe2_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h deleted file mode 100755 index f7a04d908b..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe2_types.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:28 2013 - * - */ - -#ifndef __KALMAN_DLQE2_TYPES_H__ -#define __KALMAN_DLQE2_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe2_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c deleted file mode 100755 index 9efe2ea7a4..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c +++ /dev/null @@ -1,137 +0,0 @@ -/* - * kalman_dlqe3.c - * - * Code generation for function 'kalman_dlqe3' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe3.h" -#include "randn.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f1; - real32_T f2; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f1 = (real32_T)fabs(u0); - f2 = (real32_T)fabs(u1); - if (rtIsInfF(u1)) { - if (f1 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f1 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f2 == 0.0F) { - y = 1.0F; - } else if (f2 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)pow(u0, u1); - } - } - - return y; -} - -void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const - real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, - real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]) -{ - real32_T A[9]; - int32_T i0; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real_T b; - real32_T y; - real32_T b_y[3]; - int32_T i1; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T b_k1[3]; - real32_T f0; - A[0] = 1.0F; - A[3] = dt; - A[6] = 0.5F * rt_powf_snf(dt, 2.0F); - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = dt; - for (i0 = 0; i0 < 3; i0++) { - A[2 + 3 * i0] = (real32_T)iv0[i0]; - } - - if (addNoise == 1.0F) { - b = randn(); - z += sigma * (real32_T)b; - } - - if (posUpdate != 0.0F) { - y = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - b_y[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0]; - } - - y += b_y[i0] * x_aposteriori_k[i0]; - } - - y = z - y; - b_k1[0] = k1; - b_k1[1] = k2; - b_k1[2] = k3; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - - x_aposteriori[i0] = f0 + b_k1[i0] * y; - } - } else { - for (i0 = 0; i0 < 3; i0++) { - x_aposteriori[i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1]; - } - } - } -} - -/* End of code generation (kalman_dlqe3.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h deleted file mode 100755 index 9bbffbbb30..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * kalman_dlqe3.h - * - * Code generation for function 'kalman_dlqe3' - * - * C source code generated on: Tue Feb 19 15:26:32 2013 - * - */ - -#ifndef __KALMAN_DLQE3_H__ -#define __KALMAN_DLQE3_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe3_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]); -#endif -/* End of code generation (kalman_dlqe3.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c deleted file mode 100755 index 8f2275c131..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c +++ /dev/null @@ -1,32 +0,0 @@ -/* - * kalman_dlqe3_data.c - * - * Code generation for function 'kalman_dlqe3_data' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe3.h" -#include "kalman_dlqe3_data.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ -uint32_T method; -uint32_T state[2]; -uint32_T b_method; -uint32_T b_state; -uint32_T c_state[2]; -boolean_T state_not_empty; - -/* Function Declarations */ - -/* Function Definitions */ -/* End of code generation (kalman_dlqe3_data.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h deleted file mode 100755 index 952eb7b894..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * kalman_dlqe3_data.h - * - * Code generation for function 'kalman_dlqe3_data' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -#ifndef __KALMAN_DLQE3_DATA_H__ -#define __KALMAN_DLQE3_DATA_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe3_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ -extern uint32_T method; -extern uint32_T state[2]; -extern uint32_T b_method; -extern uint32_T b_state; -extern uint32_T c_state[2]; -extern boolean_T state_not_empty; - -/* Variable Definitions */ - -/* Function Declarations */ -#endif -/* End of code generation (kalman_dlqe3_data.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c deleted file mode 100755 index b87d604c43..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c +++ /dev/null @@ -1,47 +0,0 @@ -/* - * kalman_dlqe3_initialize.c - * - * Code generation for function 'kalman_dlqe3_initialize' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe3.h" -#include "kalman_dlqe3_initialize.h" -#include "kalman_dlqe3_data.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe3_initialize(void) -{ - int32_T i; - static const uint32_T uv0[2] = { 362436069U, 0U }; - - rt_InitInfAndNaN(8U); - state_not_empty = FALSE; - b_state = 1144108930U; - b_method = 7U; - method = 0U; - for (i = 0; i < 2; i++) { - c_state[i] = 362436069U + 158852560U * (uint32_T)i; - state[i] = uv0[i]; - } - - if (state[1] == 0U) { - state[1] = 521288629U; - } -} - -/* End of code generation (kalman_dlqe3_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h deleted file mode 100755 index 9dee90f9e3..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * kalman_dlqe3_initialize.h - * - * Code generation for function 'kalman_dlqe3_initialize' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -#ifndef __KALMAN_DLQE3_INITIALIZE_H__ -#define __KALMAN_DLQE3_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe3_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe3_initialize(void); -#endif -/* End of code generation (kalman_dlqe3_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c deleted file mode 100755 index b008582053..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * kalman_dlqe3_terminate.c - * - * Code generation for function 'kalman_dlqe3_terminate' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe3.h" -#include "kalman_dlqe3_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void kalman_dlqe3_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (kalman_dlqe3_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h deleted file mode 100755 index 69cc85c765..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * kalman_dlqe3_terminate.h - * - * Code generation for function 'kalman_dlqe3_terminate' - * - * C source code generated on: Tue Feb 19 15:26:31 2013 - * - */ - -#ifndef __KALMAN_DLQE3_TERMINATE_H__ -#define __KALMAN_DLQE3_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe3_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void kalman_dlqe3_terminate(void); -#endif -/* End of code generation (kalman_dlqe3_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h deleted file mode 100755 index f36ea4557a..0000000000 --- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * kalman_dlqe3_types.h - * - * Code generation for function 'kalman_dlqe3' - * - * C source code generated on: Tue Feb 19 15:26:30 2013 - * - */ - -#ifndef __KALMAN_DLQE3_TYPES_H__ -#define __KALMAN_DLQE3_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (kalman_dlqe3_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c deleted file mode 100755 index 5139848bca..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c +++ /dev/null @@ -1,136 +0,0 @@ -/* - * positionKalmanFilter1D.c - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const - real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T - P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T - Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], - real32_T P_aposteriori[9]) -{ - int32_T i0; - real32_T f0; - int32_T k; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - real32_T y; - real32_T K[3]; - real32_T S; - int8_T I[9]; - - /* prediction */ - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += A[i0 + 3 * k] * x_aposteriori_k[k]; - } - - x_aposteriori[i0] = f0 + B[i0] * u; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k]; - } - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1]; - } - - P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - y = 0.0F; - for (k = 0; k < 3; k++) { - y += C[k] * x_aposteriori[k]; - K[k] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - K[k] += C[i0] * P_apriori[i0 + 3 * k]; - } - } - - y = z - y; - S = 0.0F; - for (k = 0; k < 3; k++) { - S += K[k] * C[k]; - } - - S += R; - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (k = 0; k < 3; k++) { - f0 += P_apriori[i0 + 3 * k] * C[k]; - } - - K[i0] = f0 / S; - } - - for (i0 = 0; i0 < 3; i0++) { - x_aposteriori[i0] += K[i0] * y; - } - - for (i0 = 0; i0 < 9; i0++) { - I[i0] = 0; - } - - for (k = 0; k < 3; k++) { - I[k + 3 * k] = 1; - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - for (k = 0; k < 3; k++) { - P_aposteriori[i0 + 3 * k] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k]; - } - } - } - } else { - for (i0 = 0; i0 < 9; i0++) { - P_aposteriori[i0] = P_apriori[i0]; - } - } -} - -/* End of code generation (positionKalmanFilter1D.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h deleted file mode 100755 index 205c8eb4ee..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_H__ -#define __POSITIONKALMANFILTER1D_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c deleted file mode 100755 index 4c535618a3..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c +++ /dev/null @@ -1,157 +0,0 @@ -/* - * positionKalmanFilter1D_dT.c - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], - const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, - const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T - x_aposteriori[3], real32_T P_aposteriori[9]) -{ - real32_T A[9]; - int32_T i; - static const int8_T iv0[3] = { 0, 0, 1 }; - - real32_T K[3]; - real32_T f0; - int32_T i0; - real32_T b_A[9]; - int32_T i1; - real32_T P_apriori[9]; - static const int8_T iv1[3] = { 1, 0, 0 }; - - real32_T fv0[3]; - real32_T y; - static const int8_T iv2[3] = { 1, 0, 0 }; - - real32_T S; - int8_T I[9]; - - /* dynamics */ - A[0] = 1.0F; - A[3] = dT; - A[6] = -0.5F * dT * dT; - A[1] = 0.0F; - A[4] = 1.0F; - A[7] = -dT; - for (i = 0; i < 3; i++) { - A[2 + 3 * i] = (real32_T)iv0[i]; - } - - /* prediction */ - K[0] = 0.5F * dT * dT; - K[1] = dT; - K[2] = 0.0F; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += A[i + 3 * i0] * x_aposteriori_k[i0]; - } - - x_aposteriori[i] = f0 + K[i] * u; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - b_A[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1]; - } - - P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0]; - } - } - - if ((real32_T)fabs(u) < thresh) { - x_aposteriori[1] *= decay; - } - - /* update */ - if (gps_update == 1) { - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += (real32_T)iv1[i] * x_aposteriori[i]; - fv0[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i]; - } - } - - y = z - f0; - f0 = 0.0F; - for (i = 0; i < 3; i++) { - f0 += fv0[i] * (real32_T)iv2[i]; - } - - S = f0 + (real32_T)R; - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0]; - } - - K[i] = f0 / S; - } - - for (i = 0; i < 3; i++) { - x_aposteriori[i] += K[i] * y; - } - - for (i = 0; i < 9; i++) { - I[i] = 0; - } - - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1; - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - P_aposteriori[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0]; - } - } - } - } else { - for (i = 0; i < 9; i++) { - P_aposteriori[i] = P_apriori[i]; - } - } -} - -/* End of code generation (positionKalmanFilter1D_dT.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h deleted file mode 100755 index 94cbe2ce62..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_H__ -#define __POSITIONKALMANFILTER1D_DT_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]); -#endif -/* End of code generation (positionKalmanFilter1D_dT.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c deleted file mode 100755 index aa89f8a9d9..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h deleted file mode 100755 index 8d358a9a35..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_dT_initialize' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c deleted file mode 100755 index 20ed2edbb6..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D_dT.h" -#include "positionKalmanFilter1D_dT_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_dT_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h deleted file mode 100755 index 5eb5807a0a..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_dT_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_dT_terminate' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_dT_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_dT_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h deleted file mode 100755 index 43e5f016cb..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_dT_types.h - * - * Code generation for function 'positionKalmanFilter1D_dT' - * - * C source code generated on: Fri Nov 30 17:37:33 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__ -#define __POSITIONKALMANFILTER1D_DT_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_dT_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c deleted file mode 100755 index 5bd87c3902..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.c - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (positionKalmanFilter1D_initialize.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h deleted file mode 100755 index 44bce472f4..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_initialize.h - * - * Code generation for function 'positionKalmanFilter1D_initialize' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__ -#define __POSITIONKALMANFILTER1D_INITIALIZE_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_initialize(void); -#endif -/* End of code generation (positionKalmanFilter1D_initialize.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c deleted file mode 100755 index 41e11936f8..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.c - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "positionKalmanFilter1D.h" -#include "positionKalmanFilter1D_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void positionKalmanFilter1D_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (positionKalmanFilter1D_terminate.c) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h deleted file mode 100755 index e84ea01bc0..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h +++ /dev/null @@ -1,31 +0,0 @@ -/* - * positionKalmanFilter1D_terminate.h - * - * Code generation for function 'positionKalmanFilter1D_terminate' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__ -#define __POSITIONKALMANFILTER1D_TERMINATE_H__ -/* Include files */ -#include -#include -#include - -#include "rtwtypes.h" -#include "positionKalmanFilter1D_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void positionKalmanFilter1D_terminate(void); -#endif -/* End of code generation (positionKalmanFilter1D_terminate.h) */ diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h deleted file mode 100755 index 4b473f56f2..0000000000 --- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * positionKalmanFilter1D_types.h - * - * Code generation for function 'positionKalmanFilter1D' - * - * C source code generated on: Fri Nov 30 14:26:11 2012 - * - */ - -#ifndef __POSITIONKALMANFILTER1D_TYPES_H__ -#define __POSITIONKALMANFILTER1D_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (positionKalmanFilter1D_types.h) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c deleted file mode 100755 index 51aef7b76d..0000000000 --- a/src/modules/position_estimator_mc/codegen/randn.c +++ /dev/null @@ -1,524 +0,0 @@ -/* - * randn.c - * - * Code generation for function 'randn' - * - * C source code generated on: Tue Feb 19 15:26:32 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "kalman_dlqe3.h" -#include "randn.h" -#include "kalman_dlqe3_data.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ -static uint32_T d_state[625]; - -/* Function Declarations */ -static real_T b_genrandu(uint32_T mt[625]); -static real_T eml_rand_mt19937ar(uint32_T e_state[625]); -static real_T eml_rand_shr3cong(uint32_T e_state[2]); -static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]); -static void genrandu(uint32_T s, uint32_T *e_state, real_T *r); -static void twister_state_vector(uint32_T mt[625], real_T seed); - -/* Function Definitions */ -static real_T b_genrandu(uint32_T mt[625]) -{ - real_T r; - int32_T exitg1; - uint32_T u[2]; - boolean_T isvalid; - int32_T k; - boolean_T exitg2; - - /* This is a uniform (0,1) pseudorandom number generator based on: */ - /* */ - /* A C-program for MT19937, with initialization improved 2002/1/26. */ - /* Coded by Takuji Nishimura and Makoto Matsumoto. */ - /* */ - /* Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */ - /* All rights reserved. */ - /* */ - /* Redistribution and use in source and binary forms, with or without */ - /* modification, are permitted provided that the following conditions */ - /* are met: */ - /* */ - /* 1. Redistributions of source code must retain the above copyright */ - /* notice, this list of conditions and the following disclaimer. */ - /* */ - /* 2. Redistributions in binary form must reproduce the above copyright */ - /* notice, this list of conditions and the following disclaimer in the */ - /* documentation and/or other materials provided with the distribution. */ - /* */ - /* 3. The names of its contributors may not be used to endorse or promote */ - /* products derived from this software without specific prior written */ - /* permission. */ - /* */ - /* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */ - /* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */ - /* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */ - /* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */ - /* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */ - /* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */ - /* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */ - /* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */ - /* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */ - /* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */ - /* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - do { - exitg1 = 0; - genrand_uint32_vector(mt, u); - r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T) - (u[1] >> 6U)); - if (r == 0.0) { - if ((mt[624] >= 1U) && (mt[624] < 625U)) { - isvalid = TRUE; - } else { - isvalid = FALSE; - } - - if (isvalid) { - isvalid = FALSE; - k = 1; - exitg2 = FALSE; - while ((exitg2 == FALSE) && (k < 625)) { - if (mt[k - 1] == 0U) { - k++; - } else { - isvalid = TRUE; - exitg2 = TRUE; - } - } - } - - if (!isvalid) { - twister_state_vector(mt, 5489.0); - } - } else { - exitg1 = 1; - } - } while (exitg1 == 0); - - return r; -} - -static real_T eml_rand_mt19937ar(uint32_T e_state[625]) -{ - real_T r; - int32_T exitg1; - uint32_T u32[2]; - int32_T i; - static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068, - 0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787, - 0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557, - 0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991, - 0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419, - 0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975, - 0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107, - 0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997, - 0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153, - 0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165, - 0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959, - 0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086, - 0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062, - 0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548, - 1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093, - 1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034, - 1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357, - 1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815, - 1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083, - 1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944, - 1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653, - 1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949, - 1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382, - 1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073, - 1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936, - 1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328, - 1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998, - 1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547, - 1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872, - 1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808, - 1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049, - 1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761, - 1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417, - 1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591, - 1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609, - 1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487, - 1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818, - 1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719, - 1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782, - 1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172, - 1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452, - 1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082, - 1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281, - 1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127, - 1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043, - 1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569, - 1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366, - 1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053, - 2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753, - 2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983, - 2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952, - 2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431, - 2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416, - 2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379, - 2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137, - 2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729, - 2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715, - 2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105, - 2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184, - 2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674, - 2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999, - 2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341, - 2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214, - 3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143, - 3.65415288536101, 3.91075795952492 }; - - real_T u; - static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108, - 0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131, - 0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857, - 0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884, - 0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964, - 0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187, - 0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157, - 0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863, - 0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512, - 0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774, - 0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697, - 0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624, - 0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914, - 0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987, - 0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354, - 0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766, - 0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952, - 0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322, - 0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468, - 0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458, - 0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773, - 0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716, - 0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804, - 0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022, - 0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188, - 0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832, - 0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393, - 0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032, - 0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077, - 0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522, - 0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601, - 0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337, - 0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803, - 0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183, - 0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668, - 0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261, - 0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942, - 0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501, - 0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681, - 0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331, - 0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881, - 0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135, - 0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268, - 0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839, - 0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366, - 0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263, - 0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514, - 0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669, - 0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597, - 0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839, - 0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487, - 0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009, - 0.093365311912691, 0.0911136480663738, 0.0888735920682759, - 0.0866451944505581, 0.0844285095703535, 0.082223595813203, - 0.0800305158146631, 0.0778493367020961, 0.0756801303589272, - 0.0735229737139814, 0.0713779490588905, 0.0692451443970068, - 0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954, - 0.0587679529209339, 0.0567106901062031, 0.0546664613248891, - 0.0526354182767924, 0.0506177238609479, 0.0486135532158687, - 0.0466230949019305, 0.0446465522512946, 0.0426841449164746, - 0.0407361106559411, 0.0388027074045262, 0.0368842156885674, - 0.0349809414617162, 0.0330932194585786, 0.0312214171919203, - 0.0293659397581334, 0.0275272356696031, 0.0257058040085489, - 0.0239022033057959, 0.0221170627073089, 0.0203510962300445, - 0.0186051212757247, 0.0168800831525432, 0.0151770883079353, - 0.0134974506017399, 0.0118427578579079, 0.0102149714397015, - 0.00861658276939875, 0.00705087547137324, 0.00552240329925101, - 0.00403797259336304, 0.00260907274610216, 0.0012602859304986, - 0.000477467764609386 }; - - real_T x; - do { - exitg1 = 0; - genrand_uint32_vector(e_state, u32); - i = (int32_T)((u32[1] >> 24U) + 1U); - r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] & - 16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i]; - if (fabs(r) <= dv1[i - 1]) { - exitg1 = 1; - } else if (i < 256) { - u = b_genrandu(e_state); - if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) { - exitg1 = 1; - } - } else { - do { - u = b_genrandu(e_state); - x = log(u) * 0.273661237329758; - u = b_genrandu(e_state); - } while (!(-2.0 * log(u) > x * x)); - - if (r < 0.0) { - r = x - 3.65415288536101; - } else { - r = 3.65415288536101 - x; - } - - exitg1 = 1; - } - } while (exitg1 == 0); - - return r; -} - -static real_T eml_rand_shr3cong(uint32_T e_state[2]) -{ - real_T r; - uint32_T icng; - uint32_T jsr; - uint32_T ui; - int32_T j; - static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427, - 0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948, - 0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738, - 1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008, - 1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122, - 1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503, - 1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831, - 1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713, - 2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658, - 2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 }; - - real_T x; - real_T y; - real_T s; - icng = 69069U * e_state[0] + 1234567U; - jsr = e_state[1] ^ e_state[1] << 13U; - jsr ^= jsr >> 17U; - jsr ^= jsr << 5U; - ui = icng + jsr; - j = (int32_T)(ui & 63U); - r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1]; - if (fabs(r) <= dv0[j]) { - } else { - x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]); - icng = 69069U * icng + 1234567U; - jsr ^= jsr << 13U; - jsr ^= jsr >> 17U; - jsr ^= jsr << 5U; - y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10; - s = x + (0.5 + y); - if (s > 1.301198) { - if (r < 0.0) { - r = 0.4878992 * x - 0.4878992; - } else { - r = 0.4878992 - 0.4878992 * x; - } - } else if (s <= 0.9689279) { - } else { - s = 0.4878992 * x; - x = 0.4878992 - 0.4878992 * x; - if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) { - if (r < 0.0) { - r = -(0.4878992 - s); - } else { - r = 0.4878992 - s; - } - } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 / - dv0[j + 1] <= exp(-0.5 * r * r)) { - } else { - do { - icng = 69069U * icng + 1234567U; - jsr ^= jsr << 13U; - jsr ^= jsr >> 17U; - jsr ^= jsr << 5U; - x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) / - 2.776994; - icng = 69069U * icng + 1234567U; - jsr ^= jsr << 13U; - jsr ^= jsr >> 17U; - jsr ^= jsr << 5U; - } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) * - 2.328306436538696E-10) > x * x)); - - if (r < 0.0) { - r = x - 2.776994; - } else { - r = 2.776994 - x; - } - } - } - } - - e_state[0] = icng; - e_state[1] = jsr; - return r; -} - -static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]) -{ - int32_T i; - uint32_T mti; - int32_T kk; - uint32_T y; - uint32_T b_y; - uint32_T c_y; - uint32_T d_y; - for (i = 0; i < 2; i++) { - u[i] = 0U; - } - - for (i = 0; i < 2; i++) { - mti = mt[624] + 1U; - if (mti >= 625U) { - for (kk = 0; kk < 227; kk++) { - y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U); - if ((int32_T)(y & 1U) == 0) { - b_y = y >> 1U; - } else { - b_y = y >> 1U ^ 2567483615U; - } - - mt[kk] = mt[397 + kk] ^ b_y; - } - - for (kk = 0; kk < 396; kk++) { - y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U); - if ((int32_T)(y & 1U) == 0) { - c_y = y >> 1U; - } else { - c_y = y >> 1U ^ 2567483615U; - } - - mt[227 + kk] = mt[kk] ^ c_y; - } - - y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U); - if ((int32_T)(y & 1U) == 0) { - d_y = y >> 1U; - } else { - d_y = y >> 1U ^ 2567483615U; - } - - mt[623] = mt[396] ^ d_y; - mti = 1U; - } - - y = mt[(int32_T)mti - 1]; - mt[624] = mti; - y ^= y >> 11U; - y ^= y << 7U & 2636928640U; - y ^= y << 15U & 4022730752U; - y ^= y >> 18U; - u[i] = y; - } -} - -static void genrandu(uint32_T s, uint32_T *e_state, real_T *r) -{ - int32_T hi; - uint32_T test1; - uint32_T test2; - hi = (int32_T)(s / 127773U); - test1 = 16807U * (s - (uint32_T)hi * 127773U); - test2 = 2836U * (uint32_T)hi; - if (test1 < test2) { - *e_state = (test1 - test2) + 2147483647U; - } else { - *e_state = test1 - test2; - } - - *r = (real_T)*e_state * 4.6566128752457969E-10; -} - -static void twister_state_vector(uint32_T mt[625], real_T seed) -{ - uint32_T r; - int32_T mti; - if (seed < 4.294967296E+9) { - if (seed >= 0.0) { - r = (uint32_T)seed; - } else { - r = 0U; - } - } else if (seed >= 4.294967296E+9) { - r = MAX_uint32_T; - } else { - r = 0U; - } - - mt[0] = r; - for (mti = 0; mti < 623; mti++) { - r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti); - mt[1 + mti] = r; - } - - mt[624] = 624U; -} - -real_T randn(void) -{ - real_T r; - uint32_T e_state; - real_T t; - real_T b_r; - uint32_T f_state; - if (method == 0U) { - if (b_method == 4U) { - do { - genrandu(b_state, &e_state, &r); - genrandu(e_state, &b_state, &t); - b_r = 2.0 * r - 1.0; - t = 2.0 * t - 1.0; - t = t * t + b_r * b_r; - } while (!(t <= 1.0)); - - r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); - } else if (b_method == 5U) { - r = eml_rand_shr3cong(c_state); - } else { - if (!state_not_empty) { - memset(&d_state[0], 0, 625U * sizeof(uint32_T)); - twister_state_vector(d_state, 5489.0); - state_not_empty = TRUE; - } - - r = eml_rand_mt19937ar(d_state); - } - } else if (method == 4U) { - e_state = state[0]; - do { - genrandu(e_state, &f_state, &r); - genrandu(f_state, &e_state, &t); - b_r = 2.0 * r - 1.0; - t = 2.0 * t - 1.0; - t = t * t + b_r * b_r; - } while (!(t <= 1.0)); - - state[0] = e_state; - r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t); - } else { - r = eml_rand_shr3cong(state); - } - - return r; -} - -/* End of code generation (randn.c) */ diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h deleted file mode 100755 index 8a2aa92778..0000000000 --- a/src/modules/position_estimator_mc/codegen/randn.h +++ /dev/null @@ -1,33 +0,0 @@ -/* - * randn.h - * - * Code generation for function 'randn' - * - * C source code generated on: Tue Feb 19 15:26:32 2013 - * - */ - -#ifndef __RANDN_H__ -#define __RANDN_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "kalman_dlqe3_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern real_T randn(void); -#endif -/* End of code generation (randn.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c deleted file mode 100755 index c6fa7884e6..0000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h deleted file mode 100755 index e7b2a2d1ca..0000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c deleted file mode 100755 index 5527701498..0000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h deleted file mode 100755 index 5acdd97901..0000000000 --- a/src/modules/position_estimator_mc/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c deleted file mode 100755 index de121c4a0c..0000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h deleted file mode 100755 index 3bbcfd0879..0000000000 --- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h deleted file mode 100755 index 8916e8572c..0000000000 --- a/src/modules/position_estimator_mc/codegen/rtwtypes.h +++ /dev/null @@ -1,159 +0,0 @@ -/* - * rtwtypes.h - * - * Code generation for function 'kalman_dlqe2' - * - * C source code generated on: Thu Feb 14 12:52:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: off - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m deleted file mode 100755 index ff939d0297..0000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe1.m +++ /dev/null @@ -1,3 +0,0 @@ -function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z) - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end \ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m deleted file mode 100755 index 2a50164efd..0000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe2.m +++ /dev/null @@ -1,9 +0,0 @@ -function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z) - st = 1/2*dt^2; - A = [1,dt,st; - 0,1,dt; - 0,0,1]; - C=[1,0,0]; - K = [k1;k2;k3]; - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); -end \ No newline at end of file diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m deleted file mode 100755 index 4c6421b7fe..0000000000 --- a/src/modules/position_estimator_mc/kalman_dlqe3.m +++ /dev/null @@ -1,17 +0,0 @@ -function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma) - st = 1/2*dt^2; - A = [1,dt,st; - 0,1,dt; - 0,0,1]; - C=[1,0,0]; - K = [k1;k2;k3]; - if addNoise==1 - noise = sigma*randn(1,1); - z = z + noise; - end - if(posUpdate) - x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k); - else - x_aposteriori=A*x_aposteriori_k; - end -end \ No newline at end of file diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk deleted file mode 100644 index 40b135ea4f..0000000000 --- a/src/modules/position_estimator_mc/module.mk +++ /dev/null @@ -1,60 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Makefile to build the position estimator -# - -MODULE_COMMAND = position_estimator_mc - -SRCS = position_estimator_mc_main.c \ - position_estimator_mc_params.c \ - codegen/positionKalmanFilter1D_initialize.c \ - codegen/positionKalmanFilter1D_terminate.c \ - codegen/positionKalmanFilter1D.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/positionKalmanFilter1D_dT_initialize.c \ - codegen/positionKalmanFilter1D_dT_terminate.c \ - codegen/kalman_dlqe1.c \ - codegen/kalman_dlqe1_initialize.c \ - codegen/kalman_dlqe1_terminate.c \ - codegen/kalman_dlqe2.c \ - codegen/kalman_dlqe2_initialize.c \ - codegen/kalman_dlqe2_terminate.c \ - codegen/kalman_dlqe3.c \ - codegen/kalman_dlqe3_initialize.c \ - codegen/kalman_dlqe3_terminate.c \ - codegen/kalman_dlqe3_data.c \ - codegen/randn.c diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m deleted file mode 100755 index 144ff8c7c2..0000000000 --- a/src/modules/position_estimator_mc/positionKalmanFilter1D.m +++ /dev/null @@ -1,19 +0,0 @@ -function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) -%prediction - x_apriori=A*x_aposteriori_k+B*u; - P_apriori=A*P_aposteriori_k*A'+Q; - if abs(u) - * Tobias Naegeli -* Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file position_estimator_main.c - * Model-identification based position estimator for multirotors - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "position_estimator_mc_params.h" -//#include -#include "codegen/kalman_dlqe2.h" -#include "codegen/kalman_dlqe2_initialize.h" -#include "codegen/kalman_dlqe3.h" -#include "codegen/kalman_dlqe3_initialize.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int position_estimator_mc_task; /**< Handle of deamon task / thread */ - -__EXPORT int position_estimator_mc_main(int argc, char *argv[]); - -int position_estimator_mc_thread_main(int argc, char *argv[]); -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - warnx("%s\n", reason); - warnx("usage: position_estimator_mc {start|stop|status}"); - exit(1); -} - -/** - * The position_estimator_mc_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int position_estimator_mc_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("position_estimator_mc already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - position_estimator_mc_task = task_spawn_cmd("position_estimator_mc", - SCHED_RR, - SCHED_PRIORITY_MAX - 5, - 4096, - position_estimator_mc_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("position_estimator_mc is running"); - } else { - warnx("position_estimator_mc not started"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -/**************************************************************************** - * main - ****************************************************************************/ -int position_estimator_mc_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("[position_estimator_mc] started"); - int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_mc] started"); - - /* initialize values */ - float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */ - // float rotMatrix[4] = {1.0f, 0.0f, 0.0f, 1.0f}; - float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f}; - float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f}; - float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f}; - - // XXX this is terribly wrong and should actual dT instead - const float dT_const_50 = 1.0f/50.0f; - - float addNoise = 0.0f; - float sigma = 0.0f; - //computed from dlqe in matlab - const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f}; - // XXX implement baro filter - const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f}; - float K[3] = {0.0f, 0.0f, 0.0f}; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ - float p0_Pa = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ - const float const_earth_gravity = 9.81f; - - float posX = 0.0f; - float posY = 0.0f; - float posZ = 0.0f; - - double lat_current; - double lon_current; - float alt_current; - - float gps_origin_altitude = 0.0f; - - /* Initialize filter */ - kalman_dlqe2_initialize(); - kalman_dlqe3_initialize(); - - /* declare and safely initialize all structs */ - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ - struct vehicle_vicon_position_s vicon_pos; - memset(&vicon_pos, 0, sizeof(vicon_pos)); - struct actuator_controls_effective_s act_eff; - memset(&act_eff, 0, sizeof(act_eff)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); - struct vehicle_global_position_s global_pos_est; - memset(&global_pos_est, 0, sizeof(global_pos_est)); - - /* subscribe */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0)); - int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* advertise */ - orb_advert_t local_pos_est_pub = 0; - orb_advert_t global_pos_est_pub = 0; - - struct position_estimator_mc_params pos1D_params; - struct position_estimator_mc_param_handles pos1D_param_handles; - /* initialize parameter handles */ - parameters_init(&pos1D_param_handles); - - bool flag_use_gps = false; - bool flag_use_baro = false; - bool flag_baro_initialized = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */ - /* FIRST PARAMETER UPDATE */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - /* END FIRST PARAMETER UPDATE */ - - /* try to grab a vicon message - if it fails, go for GPS. */ - - /* make sure the next orb_check() can't return true unless we get a timely update */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - /* allow 200 ms for vicon to come in */ - usleep(200000); - /* check if we got vicon */ - bool update_check; - orb_check(vicon_pos_sub, &update_check); - /* if no update was available, use GPS */ - flag_use_gps = !update_check; - - if (flag_use_gps) { - mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked"); - /* wait until gps signal turns valid, only then can we initialize the projection */ - - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - while (!(gps.fix_type == 3 - && (gps.eph_m < hdop_threshold_m) - && (gps.epv_m < vdop_threshold_m) - && (hrt_absolute_time() - gps.timestamp_position < 2000000))) { - - struct pollfd fds1[2] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds1, 2, 5000)) { - if (fds1[0].revents & POLLIN){ - /* Read gps position */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - } - if (fds1[1].revents & POLLIN){ - /* Read out parameters to check for an update there, e.g. useGPS variable */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - } - } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - warnx("[pos_est_mc] wait for GPS fix"); - } - printcounter++; - } - - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7d; - lon_current = ((double)(gps.lon)) * 1e-7d; - alt_current = gps.alt * 1e-3f; - gps_origin_altitude = alt_current; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ - printf("[pos_est_mc] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - - } else { - mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON"); - /* onboard calculated position estimations */ - } - thread_running = true; - - struct pollfd fds2[3] = { - { .fd = vehicle_gps_sub, .events = POLLIN }, - { .fd = vicon_pos_sub, .events = POLLIN }, - { .fd = sub_params, .events = POLLIN }, - }; - - bool vicon_updated = false; - bool gps_updated = false; - - /**< main_loop */ - while (!thread_should_exit) { - int ret = poll(fds2, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate - if (ret < 0) { - /* poll error */ - } else { - if (fds2[2].revents & POLLIN){ - /* new parameter */ - /* read from param to clear updated flag */ - struct parameter_update_s updated; - orb_copy(ORB_ID(parameter_update), sub_params, &updated); - /* update parameters */ - parameters_update(&pos1D_param_handles, &pos1D_params); - flag_use_baro = pos1D_params.baro; - sigma = pos1D_params.sigma; - addNoise = pos1D_params.addNoise; - } - vicon_updated = false; /* default is no vicon_updated */ - if (fds2[1].revents & POLLIN) { - /* new vicon position */ - orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos); - posX = vicon_pos.x; - posY = vicon_pos.y; - posZ = vicon_pos.z; - vicon_updated = true; /* set flag for vicon update */ - } /* end of poll call for vicon updates */ - gps_updated = false; - if (fds2[0].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane*/ - map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1])); - posX = z[0]; - posY = z[1]; - posZ = (float)(gps.alt * 1e-3f); - gps_updated = true; - } - - /* Main estimator loop */ - orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff); - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor); - // barometric pressure estimation at start up - if (!flag_baro_initialized){ - // mean calculation over several measurements - if (baro_loop_cnt 2) { - /* x-y-position/velocity estimation in earth frame = gps frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - } - - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) { - /* publish local position estimate */ - if (local_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - } else { - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - } - /* publish on GPS updates */ - if (gps_updated) { - - double lat, lon; - float alt = z_est + gps_origin_altitude; - - map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon); - - global_pos_est.lat = lat; - global_pos_est.lon = lon; - global_pos_est.alt = alt; - - if (global_pos_est_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est); - } else { - global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est); - } - } - } - } - } else { - /* x-y-position/velocity estimation in earth frame = vicon frame */ - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori); - memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori)); - kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori); - memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori)); - /* z-position/velocity estimation in earth frame = vicon frame */ - float z_est = 0.0f; - float local_sigma = 0.0f; - if (flag_baro_initialized && flag_use_baro) { - z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity); - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */ - local_sigma = 0.0f; /* don't add noise on barometer in any case */ - } else { - z_est = posZ; - K[0] = K_vicon_50Hz[0]; - K[1] = K_vicon_50Hz[1]; - K[2] = K_vicon_50Hz[2]; - local_sigma = sigma; - } - kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori); - memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori)); - local_pos_est.x = x_x_aposteriori_k[0]; - local_pos_est.vx = x_x_aposteriori_k[1]; - local_pos_est.y = x_y_aposteriori_k[0]; - local_pos_est.vy = x_y_aposteriori_k[1]; - local_pos_est.z = x_z_aposteriori_k[0]; - local_pos_est.vz = x_z_aposteriori_k[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - if(local_pos_est_pub > 0) - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); - else - local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); - //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); - //mavlink_log_info(mavlink_fd, buf); - } - } - } /* end of poll return value check */ - } - - printf("[pos_est_mc] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting"); - thread_running = false; - return 0; -} diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c deleted file mode 100755 index 72e5bc73b7..0000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.c +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli -* Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file position_estimator_mc_params.c - * - * Parameters for position_estimator_mc - */ - -#include "position_estimator_mc_params.h" - -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f); -PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f); -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); -PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f); - -int parameters_init(struct position_estimator_mc_param_handles *h) -{ - h->addNoise = param_find("POS_EST_ADDN"); - h->sigma = param_find("POS_EST_SIGMA"); - h->r = param_find("POS_EST_R"); - h->baro_param_handle = param_find("POS_EST_BARO"); - return OK; -} - -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p) -{ - param_get(h->addNoise, &(p->addNoise)); - param_get(h->sigma, &(p->sigma)); - param_get(h->r, &(p->R)); - param_get(h->baro_param_handle, &(p->baro)); - return OK; -} diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/position_estimator_mc/position_estimator_mc_params.h deleted file mode 100755 index c4c95f7b47..0000000000 --- a/src/modules/position_estimator_mc/position_estimator_mc_params.h +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file position_estimator_mc_params.h - * - * Parameters for Position Estimator - */ - -#include - -struct position_estimator_mc_params { - float addNoise; - float sigma; - float R; - int baro; /* consider barometer */ -}; - -struct position_estimator_mc_param_handles { - param_t addNoise; - param_t sigma; - param_t r; - param_t baro_param_handle; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct position_estimator_mc_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p); diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk deleted file mode 100644 index 89da2d8273..0000000000 --- a/src/modules/sdlog/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# sdlog Application -# - -MODULE_COMMAND = sdlog -# The main thread only buffers to RAM, needs a high priority -MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" - -SRCS = sdlog.c \ - sdlog_ringbuffer.c diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c deleted file mode 100644 index c22523bf2e..0000000000 --- a/src/modules/sdlog/sdlog.c +++ /dev/null @@ -1,840 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file sdlog.c - * @author Lorenz Meier - * - * Simple SD logger for flight data. Buffers new sensor values and - * does the heavy SD I/O in a low-priority worker thread. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include "sdlog_ringbuffer.h" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ - -static const char *mountpoint = "/fs/microsd"; -static const char *mfile_in = "/etc/logging/logconv.m"; -int sysvector_file = -1; -int mavlink_fd = -1; -struct sdlog_logbuffer lb; - -/* mutex / condition to synchronize threads */ -pthread_mutex_t sysvector_mutex; -pthread_cond_t sysvector_cond; - -/** - * System state vector log buffer writing - */ -static void *sdlog_sysvector_write_thread(void *arg); - -/** - * Create the thread to write the system vector - */ -pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf); - -/** - * SD log management function. - */ -__EXPORT int sdlog_main(int argc, char *argv[]); - -/** - * Mainloop of sd log deamon. - */ -int sdlog_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static int file_exist(const char *filename); - -static int file_copy(const char *file_old, const char *file_new); - -static void handle_command(struct vehicle_command_s *cmd); - -/** - * Print the current status. - */ -static void print_sdlog_status(void); - -/** - * Create folder for current logging session. - */ -static int create_logfolder(char *folder_path); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - errx(1, "usage: sdlog {start|stop|status} [-s ]\n\n"); -} - -// XXX turn this into a C++ class -unsigned sensor_combined_bytes = 0; -unsigned actuator_outputs_bytes = 0; -unsigned actuator_controls_bytes = 0; -unsigned sysvector_bytes = 0; -unsigned blackbox_file_bytes = 0; -uint64_t starttime = 0; - -/* logging on or off, default to true */ -bool logging_enabled = true; - -/** - * The sd log deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn_cmd(). - */ -int sdlog_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("sdlog already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn_cmd("sdlog", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 4096, - sdlog_thread_main, - (const char **)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (!thread_running) { - printf("\tsdlog is not started\n"); - } - - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - print_sdlog_status(); - - } else { - printf("\tsdlog not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int create_logfolder(char *folder_path) -{ - /* make folder on sdcard */ - uint16_t foldernumber = 1; // start with folder 0001 - int mkdir_ret; - - /* look for the next folder that does not exist */ - while (foldernumber < MAX_NO_LOGFOLDER) { - /* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */ - sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ - - if (mkdir_ret == 0) { - /* folder does not exist, success */ - - /* now copy the Matlab/Octave file */ - char mfile_out[100]; - sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); - int ret = file_copy(mfile_in, mfile_out); - - if (!ret) { - warnx("copied m file to %s", mfile_out); - - } else { - warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); - } - - break; - - } else if (mkdir_ret == -1) { - /* folder exists already */ - foldernumber++; - continue; - - } else { - warn("failed creating new folder"); - return -1; - } - } - - if (foldernumber >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warn("all %d possible folders exist already", MAX_NO_LOGFOLDER); - return -1; - } - - return 0; -} - - -static void * -sdlog_sysvector_write_thread(void *arg) -{ - /* set name */ - prctl(PR_SET_NAME, "sdlog microSD I/O", 0); - - struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg; - - int poll_count = 0; - struct sdlog_sysvector sysvect; - memset(&sysvect, 0, sizeof(sysvect)); - - while (!thread_should_exit) { - - /* make sure threads are synchronized */ - pthread_mutex_lock(&sysvector_mutex); - - /* only wait if no data is available to process */ - if (sdlog_logbuffer_is_empty(logbuf)) { - /* blocking wait for new data at this line */ - pthread_cond_wait(&sysvector_cond, &sysvector_mutex); - } - - /* only quickly load data, do heavy I/O a few lines down */ - int ret = sdlog_logbuffer_read(logbuf, &sysvect); - /* continue */ - pthread_mutex_unlock(&sysvector_mutex); - - if (ret == OK) { - sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect)); - } - - if (poll_count % 100 == 0) { - fsync(sysvector_file); - } - - poll_count++; - } - - fsync(sysvector_file); - - return OK; -} - -pthread_t -sysvector_write_start(struct sdlog_logbuffer *logbuf) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - struct sched_param param; - /* low priority, as this is expensive disk I/O */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf); - return thread; - - // XXX we have to destroy the attr at some point -} - - -int sdlog_thread_main(int argc, char *argv[]) -{ - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* log every n'th value (skip three per default) */ - int skip_value = 3; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - int ch; - - while ((ch = getopt(argc, argv, "s:r")) != EOF) { - switch (ch) { - case 's': - { - /* log only every n'th (gyro clocked) value */ - unsigned s = strtoul(optarg, NULL, 10); - - if (s < 1 || s > 250) { - errx(1, "Wrong skip value of %d, out of range (1..250)\n", s); - } else { - skip_value = s; - } - } - break; - - case 'r': - /* log only on request, disable logging per default */ - logging_enabled = false; - break; - - case '?': - if (optopt == 'c') { - warnx("Option -%c requires an argument.\n", optopt); - } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.\n", optopt); - } else { - warnx("Unknown option character `\\x%x'.\n", optopt); - } - - default: - usage("unrecognized flag"); - errx(1, "exiting."); - } - } - - if (file_exist(mountpoint) != OK) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } - - char folder_path[64]; - - if (create_logfolder(folder_path)) - errx(1, "unable to create logging folder, exiting."); - - FILE *gpsfile; - FILE *blackbox_file; - - /* string to hold the path to the sensorfile */ - char path_buf[64] = ""; - - /* only print logging path, important to find log file later */ - warnx("logging to directory %s\n", folder_path); - - /* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */ - sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector"); - - if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) { - errx(1, "opening %s failed.\n", path_buf); - } - - /* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "gps"); - - if (NULL == (gpsfile = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - int gpsfile_no = fileno(gpsfile); - - /* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */ - sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox"); - - if (NULL == (blackbox_file = fopen(path_buf, "w"))) { - errx(1, "opening %s failed.\n", path_buf); - } - - // XXX for fsync() calls - int blackbox_file_no = fileno(blackbox_file); - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - struct { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; - struct vehicle_command_s cmd; - struct vehicle_local_position_s local_pos; - struct vehicle_global_position_s global_pos; - struct vehicle_gps_position_s gps_pos; - struct vehicle_vicon_position_s vicon_pos; - struct optical_flow_s flow; - struct battery_status_s batt; - struct differential_pressure_s diff_pres; - struct airspeed_s airspeed; - } buf; - memset(&buf, 0, sizeof(buf)); - - struct { - int cmd_sub; - int sensor_sub; - int att_sub; - int spa_sub; - int act_0_sub; - int controls_0_sub; - int controls_effective_0_sub; - int local_pos_sub; - int global_pos_sub; - int gps_pos_sub; - int vicon_pos_sub; - int flow_sub; - int batt_sub; - int diff_pres_sub; - int airspeed_sub; - } subs; - - /* --- MANAGEMENT - LOGGING COMMAND --- */ - /* subscribe to ORB for vehicle command */ - subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS POSITION --- */ - /* subscribe to ORB for global position */ - subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs.sensor_sub; - /* do not rate limit, instead use skip counter (aliasing on rate limit) */ - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - fds[fdsc_count].fd = subs.att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - fds[fdsc_count].fd = subs.spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs.act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.controls_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */ - /* subscribe to ORB for actuator control */ - subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.controls_effective_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION --- */ - /* subscribe to ORB for local position */ - subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - fds[fdsc_count].fd = subs.local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION --- */ - /* subscribe to ORB for global position */ - subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - fds[fdsc_count].fd = subs.global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VICON POSITION --- */ - /* subscribe to ORB for vicon position */ - subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - fds[fdsc_count].fd = subs.vicon_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- FLOW measurements --- */ - /* subscribe to ORB for flow measurements */ - subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - fds[fdsc_count].fd = subs.flow_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- BATTERY STATUS --- */ - /* subscribe to ORB for flow measurements */ - subs.batt_sub = orb_subscribe(ORB_ID(battery_status)); - fds[fdsc_count].fd = subs.batt_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DIFFERENTIAL PRESSURE --- */ - /* subscribe to ORB for flow measurements */ - subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - fds[fdsc_count].fd = subs.diff_pres_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ - /* subscribe to ORB for airspeed */ - subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - // const int timeout = 1000; - - thread_running = true; - - /* initialize log buffer with a size of 10 */ - sdlog_logbuffer_init(&lb, 10); - - /* initialize thread synchronization */ - pthread_mutex_init(&sysvector_mutex, NULL); - pthread_cond_init(&sysvector_cond, NULL); - - /* start logbuffer emptying thread */ - pthread_t sysvector_pthread = sysvector_write_start(&lb); - - starttime = hrt_absolute_time(); - - /* track skipping */ - int skip_count = 0; - - while (!thread_should_exit) { - - /* only poll for commands, gps and sensor_combined */ - int poll_ret = poll(fds, 3, 1000); - - /* handle the poll result */ - if (poll_ret == 0) { - /* XXX this means none of our providers is giving us data - might be an error? */ - } else if (poll_ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else { - - int ifds = 0; - - /* --- VEHICLE COMMAND VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy command into local buffer */ - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - - /* always log to blackbox, even when logging disabled */ - blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d, - buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4, - (double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7); - - handle_command(&buf.cmd); - } - - /* --- VEHICLE GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps position into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - - /* if logging disabled, continue */ - if (logging_enabled) { - - /* write KML line */ - } - } - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - // /* copy sensors raw data into local buffer */ - // orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - // /* write out */ - // sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw)); - - /* always copy sensors raw data into local buffer, since poll flags won't clear else */ - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls); - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective); - orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs); - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp); - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres); - orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); - orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt); - - /* if skipping is on or logging is disabled, ignore */ - if (skip_count < skip_value || !logging_enabled) { - skip_count++; - /* do not log data */ - continue; - } else { - /* log data, reset */ - skip_count = 0; - } - - struct sdlog_sysvector sysvect = { - .timestamp = buf.raw.timestamp, - .gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]}, - .accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]}, - .mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]}, - .baro = buf.raw.baro_pres_mbar, - .baro_alt = buf.raw.baro_alt_meter, - .baro_temp = buf.raw.baro_temp_celcius, - .control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]}, - .actuators = { - buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3], - buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7] - }, - .vbat = buf.batt.voltage_v, - .bat_current = buf.batt.current_a, - .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, - .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, - .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, - .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, - .rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}, - .vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw}, - .control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]}, - .flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality}, - .diff_pressure = buf.diff_pres.differential_pressure_pa, - .ind_airspeed = buf.airspeed.indicated_airspeed_m_s, - .true_airspeed = buf.airspeed.true_airspeed_m_s - }; - - /* put into buffer for later IO */ - pthread_mutex_lock(&sysvector_mutex); - sdlog_logbuffer_write(&lb, &sysvect); - /* signal the other thread new data, but not yet unlock */ - if ((unsigned)lb.count > (lb.size / 2)) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&sysvector_cond); - } - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&sysvector_mutex); - } - - } - - } - - print_sdlog_status(); - - /* wake up write thread one last time */ - pthread_mutex_lock(&sysvector_mutex); - pthread_cond_signal(&sysvector_cond); - /* unlock, now the writer thread may return */ - pthread_mutex_unlock(&sysvector_mutex); - - /* wait for write thread to return */ - (void)pthread_join(sysvector_pthread, NULL); - - pthread_mutex_destroy(&sysvector_mutex); - pthread_cond_destroy(&sysvector_cond); - - warnx("exiting.\n\n"); - - /* finish KML file */ - // XXX - fclose(gpsfile); - fclose(blackbox_file); - - thread_running = false; - - return 0; -} - -void print_sdlog_status() -{ - unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; - float mebibytes = bytes / 1024.0f / 1024.0f; - float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f; - - warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds)); -} - -/** - * @return 0 if file exists - */ -int file_exist(const char *filename) -{ - struct stat buffer; - return stat(filename, &buffer); -} - -int file_copy(const char *file_old, const char *file_new) -{ - FILE *source, *target; - source = fopen(file_old, "r"); - int ret = 0; - - if (source == NULL) { - warnx("failed opening input file to copy"); - return 1; - } - - target = fopen(file_new, "w"); - - if (target == NULL) { - fclose(source); - warnx("failed to open output file to copy"); - return 1; - } - - char buf[128]; - int nread; - - while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) { - int ret = fwrite(buf, 1, nread, target); - - if (ret <= 0) { - warnx("error writing file"); - ret = 1; - break; - } - } - - fsync(fileno(target)); - - fclose(source); - fclose(target); - - return ret; -} - -void handle_command(struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* request to set different system mode */ - switch (cmd->command) { - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param3)) == 1) { - - /* enable logging */ - mavlink_log_info(mavlink_fd, "[log] file:"); - mavlink_log_info(mavlink_fd, "logdir"); - logging_enabled = true; - } - if (((int)(cmd->param3)) == 0) { - - /* disable logging */ - mavlink_log_info(mavlink_fd, "[log] stopped."); - logging_enabled = false; - } - break; - - default: - /* silently ignore */ - break; - } - -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.c b/src/modules/sdlog/sdlog_ringbuffer.c deleted file mode 100644 index d7c8a47596..0000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.c +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file sdlog_log.c - * MAVLink text logging. - * - * @author Lorenz Meier - */ - -#include -#include - -#include "sdlog_ringbuffer.h" - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size) -{ - lb->size = size; - lb->start = 0; - lb->count = 0; - lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector)); -} - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb) -{ - return lb->count == (int)lb->size; -} - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb) -{ - return lb->count == 0; -} - - -// XXX make these functions thread-safe -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem) -{ - int end = (lb->start + lb->count) % lb->size; - memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector)); - - if (sdlog_logbuffer_is_full(lb)) { - lb->start = (lb->start + 1) % lb->size; /* full, overwrite */ - - } else { - ++lb->count; - } -} - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem) -{ - if (!sdlog_logbuffer_is_empty(lb)) { - memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector)); - lb->start = (lb->start + 1) % lb->size; - --lb->count; - return 0; - - } else { - return 1; - } -} diff --git a/src/modules/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h deleted file mode 100644 index b65916459e..0000000000 --- a/src/modules/sdlog/sdlog_ringbuffer.h +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file sdlog_ringbuffer.h - * microSD logging - * - * @author Lorenz Meier - */ - -#ifndef SDLOG_RINGBUFFER_H_ -#define SDLOG_RINGBUFFER_H_ - -#pragma pack(push, 1) -struct sdlog_sysvector { - uint64_t timestamp; /**< time [us] */ - float gyro[3]; /**< [rad/s] */ - float accel[3]; /**< [m/s^2] */ - float mag[3]; /**< [gauss] */ - float baro; /**< pressure [millibar] */ - float baro_alt; /**< altitude above MSL [meter] */ - float baro_temp; /**< [degree celcius] */ - float control[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float actuators[8]; /**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */ - float vbat; /**< battery voltage in [volt] */ - float bat_current; /**< battery discharge current */ - float bat_discharged; /**< discharged energy in mAh */ - float adc[4]; /**< ADC ports [volt] */ - float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ - int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ - float attitude[3]; /**< roll, pitch, yaw [rad] */ - float rotMatrix[9]; /**< unitvectors */ - float vicon[6]; /**< Vicon ground truth x, y, z and roll, pitch, yaw */ - float control_effective[4]; /**< roll, pitch, yaw [-1..1], thrust [0..1] */ - float flow[6]; /**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */ - float diff_pressure; /**< differential pressure */ - float ind_airspeed; /**< indicated airspeed */ - float true_airspeed; /**< true airspeed */ -}; -#pragma pack(pop) - -struct sdlog_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct sdlog_sysvector *elems; -}; - -void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size); - -int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb); - -int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb); - -void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem); - -int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); - -#endif From 41a0f17b4e929a4563cebeff402ce5fdb02771a2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 4 May 2014 19:55:24 +0200 Subject: [PATCH 69/79] mc.defaults: MPC_TILTMAX_XXX parameters fixed --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a9..a8c6dc8117 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -32,9 +32,9 @@ then param set MPC_Z_VEL_D 0.0 param set MPC_Z_VEL_MAX 3 param set MPC_Z_FF 0.5 - param set MPC_TILT_MAX 1.0 + param set MPC_TILTMAX_AIR 45.0 + param set MPC_TILTMAX_LND 15.0 param set MPC_LAND_SPEED 1.0 - param set MPC_LAND_TILT 0.3 fi set PWM_RATE 400 From e04b8d221b43feb1b9d6e255e420e7d9670c5aa1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 4 May 2014 21:39:15 +0200 Subject: [PATCH 70/79] att_pos_estimator_ekf restored --- .../att_pos_estimator_ekf/KalmanNav.cpp | 812 ++++++++++++++++++ .../att_pos_estimator_ekf/KalmanNav.hpp | 194 +++++ .../att_pos_estimator_ekf/kalman_main.cpp | 157 ++++ src/modules/att_pos_estimator_ekf/module.mk | 42 + src/modules/att_pos_estimator_ekf/params.c | 49 ++ 5 files changed, 1254 insertions(+) create mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp create mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp create mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp create mode 100644 src/modules/att_pos_estimator_ekf/module.mk create mode 100644 src/modules/att_pos_estimator_ekf/params.c diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp new file mode 100644 index 0000000000..5cf84542b1 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -0,0 +1,812 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KalmanNav.cpp + * + * Kalman filter navigation code + */ + +#include + +#include "KalmanNav.hpp" +#include +#include + +// constants +// Titterton pg. 52 +static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s +static const float R0 = 6378137.0f; // earth radius, m +static const float g0 = 9.806f; // standard gravitational accel. m/s^2 +static const int8_t ret_ok = 0; // no error in function +static const int8_t ret_error = -1; // error occurred + +KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), + _att(&getPublications(), ORB_ID(vehicle_attitude)), + // timestamps + _pubTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), + _attTimeStamp(hrt_absolute_time()), + _outTimeStamp(hrt_absolute_time()), + // frame count + _navFrames(0), + // miss counts + _miss(0), + // accelerations + fN(0), fE(0), fD(0), + // state + phi(0), theta(0), psi(0), + vN(0), vE(0), vD(0), + lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), + // parameters for ground station + _vGyro(this, "V_GYRO"), + _vAccel(this, "V_ACCEL"), + _rMag(this, "R_MAG"), + _rGpsVel(this, "R_GPS_VEL"), + _rGpsPos(this, "R_GPS_POS"), + _rGpsAlt(this, "R_GPS_ALT"), + _rPressAlt(this, "R_PRESS_ALT"), + _rAccel(this, "R_ACCEL"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), + _attitudeInitialized(false), + _positionInitialized(false), + _attitudeInitCounter(0) +{ + using namespace math; + + memset(&ref, 0, sizeof(ref)); + + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + + // initial state covariance matrix + P0.identity(); + P0 *= 0.01f; + P = P0; + + // initial state + phi = 0.0f; + theta = 0.0f; + psi = 0.0f; + vN = 0.0f; + vE = 0.0f; + vD = 0.0f; + lat = 0.0f; + lon = 0.0f; + alt = 0.0f; + + // initialize rotation quaternion with a single raw sensor measurement + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); + + // initialize dcm + C_nb = q.to_dcm(); + + // HPos is constant + HPos(0, 3) = 1.0f; + HPos(1, 4) = 1.0f; + HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(4, 8) = 1.0f; + HPos(5, 8) = 1.0f; + + // initialize all parameters + updateParams(); +} + +math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + return math::Quaternion(q0, q1, q2, q3); + +} + +void KalmanNav::update() +{ + using namespace math; + + struct pollfd fds[1]; + fds[0].fd = _sensors.getHandle(); + fds[0].events = POLLIN; + + // poll for new data + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + // XXX this is seriously bad - should be an emergency + return; + + } else if (ret == 0) { // timeout + return; + } + + // get new timestamp + uint64_t newTimeStamp = hrt_absolute_time(); + + // check updated subscriptions + if (_param_update.updated()) updateParams(); + + bool gpsUpdate = _gps.updated(); + bool sensorsUpdate = _sensors.updated(); + + // get new information from subscriptions + // this clears update flag + updateSubscriptions(); + + // initialize attitude when sensors online + if (!_attitudeInitialized && sensorsUpdate) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; + + if (_attitudeInitCounter > 100) { + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", + double(phi), double(theta), double(psi)); + _attitudeInitialized = true; + } + } + + // initialize position when gps received + if (!_positionInitialized && + _attitudeInitialized && // wait for attitude first + gpsUpdate && + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + map_projection_init(&ref, lat0, lon0); + _positionInitialized = true; + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", + double(vN), double(vE), double(vD), + lat, lon, double(alt)); + } + + // prediction step + // using sensors timestamp so we can account for packet lag + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dt)); + _predictTimeStamp = _sensors.timestamp; + + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); + // count fast frames + _navFrames += 1; + } + + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; + + // gps correction step + if (_positionInitialized && gpsUpdate) { + correctPos(); + } + + // attitude correction step + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz + ) { + _attTimeStamp = _sensors.timestamp; + correctAtt(); + } + + // publication + if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz + _pubTimeStamp = newTimeStamp; + + updatePublications(); + } + + // output + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz + _outTimeStamp = newTimeStamp; + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); + _navFrames = 0; + _miss = 0; + } +} + +void KalmanNav::updatePublications() +{ + using namespace math; + + // global position publication + _pos.timestamp = _pubTimeStamp; + _pos.time_gps_usec = _gps.timestamp_position; + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; + _pos.alt = float(alt); + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; + _pos.yaw = psi; + + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(&ref, lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = lat * M_RAD_TO_DEG; + _localPos.ref_lon = lon * M_RAD_TO_DEG; + _localPos.ref_alt = 0; + _localPos.landed = landed; + + // attitude publication + _att.timestamp = _pubTimeStamp; + _att.roll = phi; + _att.pitch = theta; + _att.yaw = psi; + _att.rollspeed = _sensors.gyro_rad_s[0]; + _att.pitchspeed = _sensors.gyro_rad_s[1]; + _att.yawspeed = _sensors.gyro_rad_s[2]; + // TODO, add gyro offsets to filter + _att.rate_offsets[0] = 0.0f; + _att.rate_offsets[1] = 0.0f; + _att.rate_offsets[2] = 0.0f; + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = C_nb(i, j); + + for (int i = 0; i < 4; i++) _att.q[i] = q(i); + + _att.R_valid = true; + _att.q_valid = true; + + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) { + _pos.update(); + _localPos.update(); + } + + if (_attitudeInitialized) + _att.update(); +} + +int KalmanNav::predictState(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + // attitude prediction + if (_attitudeInitialized) { + Vector<3> w(_sensors.gyro_rad_s); + + // attitude + q = q + q.derivative(w) * dt; + + // renormalize quaternion if needed + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); + } + + // C_nb update + C_nb = q.to_dcm(); + + // euler update + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; + + // specific acceleration in nav frame + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; + fN = accelN(0); + fE = accelN(1); + fD = accelN(2); + } + + // position prediction + if (_positionInitialized) { + // neglects angular deflections in local gravity + // see Titerton pg. 70 + float R = R0 + float(alt); + float LDot = vN / R; + float lDot = vE / (cosLSing * R); + float rotRate = 2 * omega + lDot; + + // XXX position prediction using speed + float vNDot = fN - vE * rotRate * sinL + + vD * LDot; + float vDDot = fD - vE * rotRate * cosL - + vN * LDot + _g.get(); + float vEDot = fE + vN * rotRate * sinL + + vDDot * rotRate * cosL; + + // rectangular integration + vN += vNDot * dt; + vE += vEDot * dt; + vD += vDDot * dt; + lat += double(LDot * dt); + lon += double(lDot * dt); + alt += double(-vD * dt); + } + + return ret_ok; +} + +int KalmanNav::predictStateCovariance(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSq = cosL * cosL; + float tanL = tanf(lat); + + // prepare for matrix + float R = R0 + float(alt); + float RSq = R * R; + + // F Matrix + // Titterton pg. 291 + + F(0, 1) = -(omega * sinL + vE * tanL / R); + F(0, 2) = vN / R; + F(0, 4) = 1.0f / R; + F(0, 6) = -omega * sinL; + F(0, 8) = -vE / RSq; + + F(1, 0) = omega * sinL + vE * tanL / R; + F(1, 2) = omega * cosL + vE / R; + F(1, 3) = -1.0f / R; + F(1, 8) = vN / RSq; + + F(2, 0) = -vN / R; + F(2, 1) = -omega * cosL - vE / R; + F(2, 4) = -tanL / R; + F(2, 6) = -omega * cosL - vE / (R * cosLSq); + F(2, 8) = vE * tanL / RSq; + + F(3, 1) = -fD; + F(3, 2) = fE; + F(3, 3) = vD / R; + F(3, 4) = -2 * (omega * sinL + vE * tanL / R); + F(3, 5) = vN / R; + F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); + F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; + + F(4, 0) = fD; + F(4, 2) = -fN; + F(4, 3) = 2 * omega * sinL + vE * tanL / R; + F(4, 4) = (vN * tanL + vD) / R; + F(4, 5) = 2 * omega * cosL + vE / R; + F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + + vN * vE / (R * cosLSq); + F(4, 8) = -vE * (vN * tanL + vD) / RSq; + + F(5, 0) = -fE; + F(5, 1) = fN; + F(5, 3) = -2 * vN / R; + F(5, 4) = -2 * (omega * cosL + vE / R); + F(5, 6) = 2 * omega * vE * sinL; + F(5, 8) = (vN * vN + vE * vE) / RSq; + + F(6, 3) = 1 / R; + F(6, 8) = -vN / RSq; + + F(7, 4) = 1 / (R * cosL); + F(7, 6) = vE * tanL / (R * cosL); + F(7, 8) = -vE / (cosL * RSq); + + F(8, 5) = -1; + + // G Matrix + // Titterton pg. 291 + G(0, 0) = -C_nb(0, 0); + G(0, 1) = -C_nb(0, 1); + G(0, 2) = -C_nb(0, 2); + G(1, 0) = -C_nb(1, 0); + G(1, 1) = -C_nb(1, 1); + G(1, 2) = -C_nb(1, 2); + G(2, 0) = -C_nb(2, 0); + G(2, 1) = -C_nb(2, 1); + G(2, 2) = -C_nb(2, 2); + + G(3, 3) = C_nb(0, 0); + G(3, 4) = C_nb(0, 1); + G(3, 5) = C_nb(0, 2); + G(4, 3) = C_nb(1, 0); + G(4, 4) = C_nb(1, 1); + G(4, 5) = C_nb(1, 2); + G(5, 3) = C_nb(2, 0); + G(5, 4) = C_nb(2, 1); + G(5, 5) = C_nb(2, 2); + + // continuous prediction equations + // for discrete time EKF + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; + + return ret_ok; +} + +int KalmanNav::correctAtt() +{ + using namespace math; + + // trig + float cosPhi = cosf(phi); + float cosTheta = cosf(theta); + // float cosPsi = cosf(psi); + float sinPhi = sinf(phi); + float sinTheta = sinf(theta); + // float sinPsi = sinf(psi); + + // mag predicted measurement + // choosing some typical magnetic field properties, + // TODO dip/dec depend on lat/ lon/ time + //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + + // compensate roll and pitch, but not yaw + // XXX take the vectors out of the C_nb matrix to avoid singularities + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); + + // mag measurement + Vector<3> magBody(_sensors.magnetometer_ga); + + // transform to earth frame + Vector<3> magNav = C_rp * magBody; + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; + if (yMag > M_PI_F) yMag -= 2*M_PI_F; + if (yMag < -M_PI_F) yMag += 2*M_PI_F; + + // accel measurement + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); + + // ignore accel correction when accel mag not close to g + Matrix<4,4> RAttAdjust = RAtt; + + bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; + + if (ignoreAccel) { + RAttAdjust(1, 1) = 1.0e10; + RAttAdjust(2, 2) = 1.0e10; + RAttAdjust(3, 3) = 1.0e10; + + } else { + //printf("correcting attitude with accel\n"); + } + + // accel predicted measurement + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); + + // calculate residual + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); + + // HMag + HAtt(0, 2) = 1; + + // HAccel + HAtt(1, 1) = cosTheta; + HAtt(2, 0) = -cosPhi * cosTheta; + HAtt(2, 1) = sinPhi * sinTheta; + HAtt(3, 0) = sinPhi * cosTheta; + HAtt(3, 1) = cosPhi * sinTheta; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; + + // check correciton is sane + for (size_t i = 0; i < xCorrect.get_size(); i++) { + float val = xCorrect(i); + + if (isnan(val) || isinf(val)) { + // abort correction and return + warnx("numerical failure in att correction"); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + if (!ignoreAccel) { + phi += xCorrect(PHI); + theta += xCorrect(THETA); + } + + psi += xCorrect(PSI); + + // attitude also affects nav velocities + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HAtt * P; + + // fault detection + float beta = y * (S.inversed() * y); + + if (beta > _faultAtt.get()) { + warnx("fault in attitude: beta = %8.4f", (double)beta); + warnx("y:"); y.print(); + } + + // update quaternions from euler + // angle correction + q.from_euler(phi, theta, psi); + + return ret_ok; +} + +int KalmanNav::correctPos() +{ + using namespace math; + + // residual + Vector<6> y; + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; + y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; + y(4) = _gps.alt / 1.0e3f - alt; + y(5) = _sensors.baro_alt_meter - alt; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; + + // check correction is sane + for (size_t i = 0; i < xCorrect.get_size(); i++) { + float val = xCorrect(i); + + if (!isfinite(val)) { + // abort correction and return + warnx("numerical failure in gps correction"); + // fallback to GPS + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + lat += double(xCorrect(LAT)); + lon += double(xCorrect(LON)); + alt += xCorrect(ALT); + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HPos * P; + + // fault detetcion + float beta = y * (S.inversed() * y); + + static int counter = 0; + if (beta > _faultPos.get() && (counter % 10 == 0)) { + warnx("fault in gps: beta = %8.4f", (double)beta); + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", + double(y(0) / sqrtf(RPos(0, 0))), + double(y(1) / sqrtf(RPos(1, 1))), + double(y(2) / sqrtf(RPos(2, 2))), + double(y(3) / sqrtf(RPos(3, 3))), + double(y(4) / sqrtf(RPos(4, 4))), + double(y(5) / sqrtf(RPos(5, 5)))); + } + counter++; + + return ret_ok; +} + +void KalmanNav::updateParams() +{ + using namespace math; + using namespace control; + SuperBlock::updateParams(); + + // gyro noise + V(0, 0) = _vGyro.get(); // gyro x, rad/s + V(1, 1) = _vGyro.get(); // gyro y + V(2, 2) = _vGyro.get(); // gyro z + + // accel noise + V(3, 3) = _vAccel.get(); // accel x, m/s^2 + V(4, 4) = _vAccel.get(); // accel y + V(5, 5) = _vAccel.get(); // accel z + + // magnetometer noise + float noiseMin = 1e-6f; + float noiseMagSq = _rMag.get() * _rMag.get(); + + if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; + + RAtt(0, 0) = noiseMagSq; // normalized direction + + // accelerometer noise + float noiseAccelSq = _rAccel.get() * _rAccel.get(); + + // bound noise to prevent singularities + if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; + + RAtt(1, 1) = noiseAccelSq; // normalized direction + RAtt(2, 2) = noiseAccelSq; + RAtt(3, 3) = noiseAccelSq; + + // gps noise + float R = R0 + float(alt); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + float noiseVel = _rGpsVel.get(); + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + float noiseGpsAlt = _rGpsAlt.get(); + float noisePressAlt = _rPressAlt.get(); + + // bound noise to prevent singularities + if (noiseVel < noiseMin) noiseVel = noiseMin; + + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + + if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; + + if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; + + RPos(0, 0) = noiseVel * noiseVel; // vn + RPos(1, 1) = noiseVel * noiseVel; // ve + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon + RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h + RPos(5, 5) = noisePressAlt * noisePressAlt; // h + // XXX, note that RPos depends on lat, so updateParams should + // be called if lat changes significantly +} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp new file mode 100644 index 0000000000..24df153cb4 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KalmanNav.hpp + * + * kalman filter navigation code + */ + +#pragma once + +//#define MATRIX_ASSERT +//#define VECTOR_ASSERT + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +/** + * Kalman filter navigation class + * http://en.wikipedia.org/wiki/Extended_Kalman_filter + * Discrete-time extended Kalman filter + */ +class KalmanNav : public control::SuperBlock +{ +public: + /** + * Constructor + */ + KalmanNav(SuperBlock *parent, const char *name); + + /** + * Deconstuctor + */ + + virtual ~KalmanNav() {}; + + math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); + + /** + * The main callback function for the class + */ + void update(); + + + /** + * Publication update + */ + virtual void updatePublications(); + + /** + * State prediction + * Continuous, non-linear + */ + int predictState(float dt); + + /** + * State covariance prediction + * Continuous, linear + */ + int predictStateCovariance(float dt); + + /** + * Attitude correction + */ + int correctAtt(); + + /** + * Position correction + */ + int correctPos(); + + /** + * Overloaded update parameters + */ + virtual void updateParams(); +protected: + // kalman filter + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ + // attitude + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + uORB::Subscription _sensors; /**< sensors sub. */ + uORB::Subscription _gps; /**< gps sub. */ + uORB::Subscription _param_update; /**< parameter update sub. */ + // publications + uORB::Publication _pos; /**< position pub. */ + uORB::Publication _localPos; /**< local position pub. */ + uORB::Publication _att; /**< attitude pub. */ + // time stamps + uint64_t _pubTimeStamp; /**< output data publication time stamp */ + uint64_t _predictTimeStamp; /**< prediction time stamp */ + uint64_t _attTimeStamp; /**< attitude correction time stamp */ + uint64_t _outTimeStamp; /**< output time stamp */ + // frame count + uint16_t _navFrames; /**< navigation frames completed in output cycle */ + // miss counts + uint16_t _miss; /**< number of times fast prediction loop missed */ + // accelerations + float fN, fE, fD; /**< navigation frame acceleration */ + // states + enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ + float phi, theta, psi; /**< 3-2-1 euler angles */ + float vN, vE, vD; /**< navigation velocity, m/s */ + double lat, lon; /**< lat, lon radians */ + // parameters + float alt; /**< altitude, meters */ + double lat0, lon0; /**< reference latitude and longitude */ + struct map_projection_reference_s ref; /**< local projection reference */ + float alt0; /**< refeerence altitude (ground height) */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ + // status + bool _attitudeInitialized; + bool _positionInitialized; + uint16_t _attitudeInitCounter; + // accessors + int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } + void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } + int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } + void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } + int32_t getAltE3() { return int32_t(alt * 1.0e3); } + void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } +}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp new file mode 100644 index 0000000000..3d20d4d2d6 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -0,0 +1,157 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file kalman_main.cpp + * Combined attitude / position estimator. + * + * @author James Goppert + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KalmanNav.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int daemon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int kalman_demo_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p ]"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int att_pos_estimator_ekf_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + + daemon_task = task_spawn_cmd("att_pos_estimator_ekf", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 30, + 8192, + kalman_demo_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running\n"); + exit(0); + + } else { + warnx("not started\n"); + exit(1); + } + + } + + usage("unrecognized command"); + exit(1); +} + +int kalman_demo_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace math; + + thread_running = true; + + KalmanNav nav(NULL, "KF"); + + while (!thread_should_exit) { + nav.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk new file mode 100644 index 0000000000..8d4a40d950 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Full attitude / position Extended Kalman Filter +# + +MODULE_COMMAND = att_pos_estimator_ekf + +SRCS = kalman_main.cpp \ + KalmanNav.cpp \ + params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c new file mode 100644 index 0000000000..4af5edeade --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/params.c @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); +PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); +PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); From 58ae1edc8447628040c8877addc485d41f802ee7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 5 May 2014 17:10:33 +0200 Subject: [PATCH 71/79] ignore .orig files --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index c992dbf5a5..afd63e06a0 100644 --- a/.gitignore +++ b/.gitignore @@ -38,3 +38,4 @@ tags .tags_sorted_by_file .pydevproject .ropeproject +*.orig From df6a0d5a1a4f528e5ba22747d7a4587b7a2263c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 12:55:39 +0200 Subject: [PATCH 72/79] mavlink: Only send the distance sensor message if the topic actually updates --- src/modules/mavlink/mavlink_messages.cpp | 27 ++++++++++++------------ 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bef8a5a55e..9c552515d3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1339,22 +1339,23 @@ protected: void send(const hrt_abstime t) { - (void)range_sub->update(t); + if (range_sub->update(t)) { - uint8_t type; + uint8_t type; - switch (range->type) { - case RANGE_FINDER_TYPE_LASER: - type = MAV_DISTANCE_SENSOR_LASER; - break; + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); } - - uint8_t id = 0; - uint8_t orientation = 0; - uint8_t covariance = 20; - - mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, - range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); } }; From 1d6b9fae037422f4c61bdd7ee1a5ea0803a59726 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 14:57:06 +0200 Subject: [PATCH 73/79] Fix in-air restarts, protect against an external MAVLink sender exploiting the restart mechanism --- src/drivers/px4io/px4io.cpp | 24 ++++++++++++++++++++---- src/modules/commander/commander.cpp | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 6 ++++++ 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8458c2fdb3..aec6dd3b75 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -683,6 +683,25 @@ PX4IO::init() /* send command to arm system via command API */ vehicle_command_s cmd; + /* send this to itself */ + param_t sys_id_param = param_find("MAV_SYS_ID"); + param_t comp_id_param = param_find("MAV_COMP_ID"); + + int32_t sys_id; + int32_t comp_id; + + if (param_get(sys_id_param, &sys_id)) { + errx(1, "PRM SYSID"); + } + + if (param_get(comp_id_param, &comp_id)) { + errx(1, "PRM CMPID"); + } + + cmd.target_system = sys_id; + cmd.target_component = comp_id; + cmd.source_system = sys_id; + cmd.source_component = comp_id; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; @@ -692,10 +711,7 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - // cmd.target_system = status.system_id; - // cmd.target_component = status.component_id; - // cmd.source_system = status.system_id; - // cmd.source_component = status.component_id; + /* ask to confirm command */ cmd.confirmation = 1; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 53ed34f46b..141b371b34 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -484,6 +484,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); } else { + + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + } transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c93c1c009..64fc41838d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP ID"); + return; + } + struct vehicle_command_s vcmd; memset(&vcmd, 0, sizeof(vcmd)); From 32b84b9652b5636ba4a2a30caae6fffacb47f1d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 21:46:59 +0200 Subject: [PATCH 74/79] Much better failsafe reporting on failsafe state changes --- src/modules/commander/commander.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 141b371b34..99a644c9d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1271,6 +1271,13 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); + } + + } else if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); } } @@ -1281,15 +1288,25 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_landed) { /* vehicle is not landed, try to perform RTL */ res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND"); + } } if (res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate) or not wanted, try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); + } + if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); } } } From 319ce3de1005d7199a992b227c7e75acb9db376b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:40:23 +0200 Subject: [PATCH 75/79] Minor cleanups in EKF estimator --- .../fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 40729e94b1..e563b4b87c 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -625,8 +625,8 @@ FixedwingEstimator::task_main() /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { - /* system is in HIL now, we got plenty of time - make sure real sensors are off */ - usleep(250000); + /* system is in HIL now, wait for measurements to come in one last round */ + usleep(65000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -1399,13 +1399,13 @@ int FixedwingEstimator::trip_nan() { _ekf->states[5] = nan_val; usleep(100000); - // warnx("tripping covariance #1 with NaN values"); - // KH[2][2] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #1 with NaN values"); + _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates + usleep(100000); - // warnx("tripping covariance #2 with NaN values"); - // KHP[5][5] = nan_val; // intermediate result used for covariance updates - // usleep(100000); + warnx("tripping covariance #2 with NaN values"); + _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates + usleep(100000); warnx("tripping covariance #3 with NaN values"); _ekf->P[3][3] = nan_val; // covariance matrix From 0c318f9ad7ab9471786a1f0fa29a599f65b5d549 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:42:57 +0200 Subject: [PATCH 76/79] commander: Fix the position failsafe to a) use proper logic to determine if eph / epv are good or not (the previous logic was in some states not initialized) and b) add a hysteresis time - because the check as it was before had zero hysteresis time for a bad eph / epv value --- src/modules/commander/commander.cpp | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 141b371b34..aa4e643dc8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,9 +117,10 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ -#define RC_TIMEOUT 500000 -#define DIFFPRESS_TIMEOUT 2000000 +#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ +#define RC_TIMEOUT 500000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -746,8 +747,9 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - uint64_t last_idle_time = 0; - uint64_t start_time = 0; + hrt_abstime last_idle_time = 0; + hrt_abstime start_time = 0; + hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -776,6 +778,9 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); + /* Init EPH and EPV */ + global_position.eph = 1000.0f; + global_position.epv = 1000.0f; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -938,11 +943,15 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { eph_epv_good = false; + } else { + eph_epv_good = true; } } else { if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { eph_epv_good = true; + } else { + eph_epv_good = false; } } check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); @@ -1264,7 +1273,12 @@ int commander_thread_main(int argc, char *argv[]) /* check if AUTO mode still allowed */ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { + if (res == TRANSITION_NOT_CHANGED) { + last_auto_state_valid = hrt_absolute_time(); + } + + /* still invalid state after the timeout interval, execute failsafe */ + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); From 2d38d113e139fa0cb448c4c6588d86d0ae878c7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:12:48 +0200 Subject: [PATCH 77/79] Fix MAVLink to use info warn level for text messages --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7ecca0d65e..833de5a3db 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1535,6 +1535,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; + statustext.severity = MAV_SEVERITY_INFO; + int i = 0; while (i < len - 1) { From 21edf72779286bbfb3c4c6bb4acad5c353300024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 14:15:11 +0200 Subject: [PATCH 78/79] Do not send a critical message when switching to dynamic state --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index e563b4b87c..05cc39f11b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -936,7 +936,7 @@ FixedwingEstimator::task_main() { const char* str = "switching to dynamic state"; warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); break; } From 23937150bce38463612ac170803a06a3424d480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 18:07:57 +0200 Subject: [PATCH 79/79] Fixed re-initialization of estimator, re-initializes in air now reliably. Does give useful HIL results. --- .../ekf_att_pos_estimator/estimator.cpp | 85 ++++++++-------- .../fw_att_pos_estimator_main.cpp | 96 ++++++++++++------- 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 86b7efafb3..a6d6a9db55 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -138,35 +138,15 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() : - fusionModeGPS(0), - covSkipCount(0), - statesInitialised(false), - fuseVelData(false), - fusePosData(false), - fuseHgtData(false), - fuseMagData(false), - fuseVtasData(false), - onGround(true), - staticMode(true), - useAirspeed(true), - useCompass(true), - useRangeFinder(true), - numericalProtection(true), - refSet(false), - storeIndex(0), - gpsHgt(0.0f), - baroHgt(0.0f), - GPSstatus(0), - VtasMeas(0.0f), - magDeclination(0.0f) -{ - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; +AttPosEKF::AttPosEKF() + + /* NOTE: DO NOT initialize class members here. Use ZeroVariables() + * instead to allow clean in-air re-initialization. + */ +{ - InitialiseParameters(); ZeroVariables(); + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -2348,7 +2328,7 @@ int AttPosEKF::CheckAndBound() } // Reset the filter if the IMU data is too old - if (dtIMU > 0.2f) { + if (dtIMU > 0.3f) { ResetVelocity(); ResetPosition(); @@ -2424,24 +2404,10 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { - // Clear the init flag - statesInitialised = false; - - // Clear other flags, waiting for new data - fusionModeGPS = 0; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - // onGround(true), - // staticMode(true), - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - - ZeroVariables(); - + // Fill variables with valid data + velNED[0] = initvelNED[0]; + velNED[1] = initvelNED[1]; + velNED[2] = initvelNED[2]; magDeclination = declination; // Calculate initial filter quaternion states from raw measurements @@ -2513,6 +2479,30 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do void AttPosEKF::ZeroVariables() { + + // Initialize on-init initialized variables + fusionModeGPS = 0; + covSkipCount = 0; + statesInitialised = false; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + // Do the data structure init for (unsigned i = 0; i < n_states; i++) { for (unsigned j = 0; j < n_states; j++) { @@ -2529,6 +2519,9 @@ void AttPosEKF::ZeroVariables() summedDelAng.zero(); summedDelVel.zero(); + dAngIMU.zero(); + dVelIMU.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 05cc39f11b..07dfdbd682 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -546,24 +546,8 @@ FixedwingEstimator::task_main() /* sets also parameters in the EKF object */ parameters_update(); - /* set initial filter state */ - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - _ekf->fuseHgtData = false; - _ekf->fuseMagData = false; - _ekf->fuseVtasData = false; - _ekf->statesInitialised = false; - - /* initialize measurement data */ - _ekf->VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; - _ekf->dVelIMU.x = 0.0f; - _ekf->dVelIMU.y = 0.0f; - _ekf->dVelIMU.z = 0.0f; - _ekf->dAngIMU.x = 0.0f; - _ekf->dAngIMU.y = 0.0f; - _ekf->dAngIMU.z = 0.0f; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; /* wakeup source(s) */ struct pollfd fds[2]; @@ -621,12 +605,14 @@ FixedwingEstimator::task_main() bool accel_updated; bool mag_updated; + hrt_abstime last_sensor_timestamp; + perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { /* system is in HIL now, wait for measurements to come in one last round */ - usleep(65000); + usleep(60000); #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -644,8 +630,13 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; - /* now skip this loop and get data on the next one */ + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; } @@ -653,8 +644,6 @@ FixedwingEstimator::task_main() * PART ONE: COLLECT ALL DATA **/ - hrt_abstime last_sensor_timestamp; - /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -668,7 +657,7 @@ FixedwingEstimator::task_main() } last_sensor_timestamp = _gyro.timestamp; - _ekf.IMUmsec = _gyro.timestamp / 1e3f; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -689,6 +678,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _gyro.x; _ekf->angRate.y = _gyro.y; _ekf->angRate.z = _gyro.z; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -696,6 +690,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; _ekf->accel.z = _accel.z; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -741,6 +740,11 @@ FixedwingEstimator::task_main() _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + + if (!_gyro_valid) { + lastAngRate = _ekf->angRate; + } + _gyro_valid = true; } @@ -748,6 +752,11 @@ FixedwingEstimator::task_main() _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + + if (!_accel_valid) { + lastAccel = _ekf->accel; + } + _accel_valid = true; } @@ -986,6 +995,22 @@ FixedwingEstimator::task_main() _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + /* set sensors to de-initialized state */ + _gyro_valid = false; + _accel_valid = false; + _mag_valid = false; + + _baro_init = false; + _gps_initialized = false; + last_sensor_timestamp = hrt_absolute_time(); + last_run = last_sensor_timestamp; + + _ekf->ZeroVariables(); + _ekf->dtIMU = 0.01f; + + // Let the system re-initialize itself + continue; + } @@ -995,15 +1020,13 @@ FixedwingEstimator::task_main() if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { - // bool home_set; - // orb_check(_home_sub, &home_set); - // struct home_position_s home; - // orb_copy(ORB_ID(home_position), _home_sub, &home); + float initVelNED[3]; if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; + + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; @@ -1018,9 +1041,6 @@ FixedwingEstimator::task_main() // Set up position variables correctly _ekf->GPSstatus = _gps.fix_type; - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; _ekf->gpsLat = math::radians(lat); _ekf->gpsLon = math::radians(lon) - M_PI; @@ -1029,7 +1049,7 @@ FixedwingEstimator::task_main() // Look up mag declination based on current position float declination = math::radians(get_mag_declination(lat, lon)); - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); + _ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = lat; @@ -1041,21 +1061,23 @@ FixedwingEstimator::task_main() mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("BARO: %8.4f m / ref: %8.4f m", _ekf->baroHgt, _ekf->hgtMea); warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); _gps_initialized = true; } else if (!_ekf->statesInitialised) { - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; + + initVelNED[0] = 0.0f; + initVelNED[1] = 0.0f; + initVelNED[2] = 0.0f; _ekf->posNED[0] = 0.0f; _ekf->posNED[1] = 0.0f; _ekf->posNED[2] = 0.0f; _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); + _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); } }