diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 12:59:47 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-04-27 12:59:47 +0200 |
commit | e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629 (patch) | |
tree | 1ae45990d97e3bd1d65574380ae1eeab5e41ff07 /src | |
parent | 574e76532126fea8ab0ac5fd0595f6fb2935f0dd (diff) | |
download | px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.tar.gz px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.tar.bz2 px4-firmware-e6ed8268ee610d0b9e9b4930ad379a6d7dcc3629.zip |
Moved position_estimator_mc, px4io driver and sdlog app to new style build
Diffstat (limited to 'src')
67 files changed, 7074 insertions, 8 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index c7f433dd4..98098c83b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Driver for the ST L3GD20 MEMS gyro connected via SPI. + * @file l3gd20.cpp + * Driver for the ST L3GD20 MEMS gyro connected via SPI. */ #include <nuttx/config.h> diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk new file mode 100644 index 000000000..328e5a684 --- /dev/null +++ b/src/drivers/px4io/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Interface driver for the PX4IO board. +# + +MODULE_COMMAND = px4io + +SRCS = px4io.cpp \ + uploader.cpp diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp new file mode 100644 index 000000000..b21f83e44 --- /dev/null +++ b/src/drivers/px4io/px4io.cpp @@ -0,0 +1,1793 @@ +/**************************************************************************** + * + * 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 px4io.cpp + * Driver for the PX4IO board. + * + * PX4IO is connected via I2C. + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <debug.h> +#include <time.h> +#include <queue.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <math.h> + +#include <arch/board/board.h> + +#include <drivers/device/device.h> +#include <drivers/device/i2c.h> +#include <drivers/drv_rc_input.h> +#include <drivers/drv_pwm_output.h> +#include <drivers/drv_gpio.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_mixer.h> + +#include <systemlib/mixer/mixer.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <systemlib/scheduling_priorities.h> +#include <systemlib/param/param.h> + +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/parameter_update.h> + +#include <px4io/protocol.h> +#include <mavlink/mavlink_log.h> +#include "uploader.h" +#include <debug.h> + +#define PX4IO_SET_DEBUG _IOC(0xff00, 0) +#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) + +class PX4IO : public device::I2C +{ +public: + PX4IO(); + virtual ~PX4IO(); + + virtual int init(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); + + /** + * Set the update rate for actuator outputs from FMU to IO. + * + * @param rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz + */ + int set_update_rate(int rate); + + /** + * Print the current status of IO + */ + void print_status(); + +private: + // XXX + unsigned _max_actuators; + unsigned _max_controls; + unsigned _max_rc_input; + unsigned _max_relays; + unsigned _max_transfer; + + unsigned _update_interval; ///< subscription interval limiting send rate + + volatile int _task; ///< worker task + volatile bool _task_should_exit; + + int _mavlink_fd; + + perf_counter_t _perf_update; + + /* cached IO state */ + uint16_t _status; + uint16_t _alarms; + + /* subscribed topics */ + int _t_actuators; ///< actuator controls topic + int _t_armed; ///< system armed control topic + int _t_vstatus; ///< system / vehicle status + int _t_param; ///< parameter update topic + + /* advertised topics */ + orb_advert_t _to_input_rc; ///< rc inputs from io + orb_advert_t _to_actuators_effective; ///< effective actuator controls topic + orb_advert_t _to_outputs; ///< mixed servo outputs topic + orb_advert_t _to_battery; ///< battery status / voltage + + actuator_outputs_s _outputs; ///< mixed outputs + actuator_controls_effective_s _controls_effective; ///< effective controls + + bool _primary_pwm_device; ///< true if we are the default PWM output + + + /** + * Trampoline to the worker task + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * worker task + */ + void task_main(); + + /** + * Send controls to IO + */ + int io_set_control_state(); + + /** + * Update IO's arming-related state + */ + int io_set_arming_state(); + + /** + * Push RC channel configuration to IO. + */ + int io_set_rc_config(); + + /** + * Fetch status and alarms from IO + * + * Also publishes battery voltage/current. + */ + int io_get_status(); + + /** + * Fetch RC inputs from IO. + * + * @param input_rc Input structure to populate. + * @return OK if data was returned. + */ + int io_get_raw_rc_input(rc_input_values &input_rc); + + /** + * Fetch and publish raw RC input data. + */ + int io_publish_raw_rc(); + + /** + * Fetch and publish the mixed control values. + */ + int io_publish_mixed_controls(); + + /** + * Fetch and publish the PWM servo outputs. + */ + int io_publish_pwm_outputs(); + + /** + * write register(s) + * + * @param page Register page to write to. + * @param offset Register offset to start writing at. + * @param values Pointer to array of values to write. + * @param num_values The number of values to write. + * @return Zero if all values were successfully written. + */ + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); + + /** + * write a register + * + * @param page Register page to write to. + * @param offset Register offset to write to. + * @param value Value to write. + * @return Zero if the value was written successfully. + */ + int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value); + + /** + * read register(s) + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @param values Pointer to array where values should be stored. + * @param num_values The number of values to read. + * @return Zero if all values were successfully read. + */ + int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values); + + /** + * read a register + * + * @param page Register page to read from. + * @param offset Register offset to start reading from. + * @return Register value that was read, or _io_reg_get_error on error. + */ + uint32_t io_reg_get(uint8_t page, uint8_t offset); + static const uint32_t _io_reg_get_error = 0x80000000; + + /** + * modify a register + * + * @param page Register page to modify. + * @param offset Register offset to modify. + * @param clearbits Bits to clear in the register. + * @param setbits Bits to set in the register. + */ + int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits); + + /** + * Send mixer definition text to IO + */ + int mixer_send(const char *buf, unsigned buflen); + + /** + * Handle a status update from IO. + * + * Publish IO status information if necessary. + * + * @param status The status register + */ + int io_handle_status(uint16_t status); + + /** + * Handle an alarm update from IO. + * + * Publish IO alarm information if necessary. + * + * @param alarm The status register + */ + int io_handle_alarms(uint16_t alarms); + +}; + + +namespace +{ + +PX4IO *g_dev; + +} + +PX4IO::PX4IO() : + I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + _max_actuators(0), + _max_controls(0), + _max_rc_input(0), + _max_relays(0), + _max_transfer(16), /* sensible default */ + _update_interval(0), + _task(-1), + _task_should_exit(false), + _mavlink_fd(-1), + _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _status(0), + _alarms(0), + _t_actuators(-1), + _t_armed(-1), + _t_vstatus(-1), + _t_param(-1), + _to_input_rc(0), + _to_actuators_effective(0), + _to_outputs(0), + _to_battery(0), + _primary_pwm_device(false) +{ + /* we need this potentially before it could be set in task_main */ + g_dev = this; + + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + + _debug_enabled = true; +} + +PX4IO::~PX4IO() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + task_delete(_task); + + g_dev = nullptr; +} + +int +PX4IO::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = I2C::init(); + if (ret != OK) + return ret; + + /* + * Enable a couple of retries for operations to IO. + * + * Register read/write operations are intentionally idempotent + * so this is safe as designed. + */ + _retries = 2; + + /* get some parameters */ + _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); + _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); + _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; + _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators > 255) || + (_max_relays < 1) || (_max_relays > 255) || + (_max_transfer < 16) || (_max_transfer > 255) || + (_max_rc_input < 1) || (_max_rc_input > 255)) { + + log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); + return -1; + } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) + _max_rc_input = RC_INPUT_MAX_CHANNELS; + + /* + * Check for IO flight state - if FMU was flagged to be in + * armed state, FMU is recovering from an in-air reset. + * Read back status and request the commander to arm + * in this case. + */ + + uint16_t reg; + + /* get IO's last seen FMU state */ + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); + if (ret != OK) + return ret; + + /* + * in-air restart is only tried if the IO board reports it is + * already armed, and has been configured for in-air restart + */ + if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + + /* WARNING: COMMANDER app/vehicle status must be initialized. + * If this fails (or the app is not started), worst-case IO + * remains untouched (so manual override is still available). + */ + + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* fill with initial values, clear updated flag */ + vehicle_status_s status; + uint64_t try_start_time = hrt_absolute_time(); + bool updated = false; + + /* keep checking for an update, ensure we got a recent state, + not something that was published a long time ago. */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + /* got data, copy and exit loop */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + break; + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + log("failed to recover from in-air restart (1), aborting IO driver init."); + return 1; + } + + } while (true); + + /* send command to arm system via command API */ + vehicle_command_s cmd; + /* request arming */ + cmd.param1 = 1.0f; + cmd.param2 = 0; + cmd.param3 = 0; + cmd.param4 = 0; + cmd.param5 = 0; + 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; + + /* send command once */ + (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + + /* spin here until IO's state has propagated into the system */ + do { + orb_check(vstatus_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + } + + /* wait 10 ms */ + usleep(10000); + + /* abort after 5s */ + if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + log("failed to recover from in-air restart (2), aborting IO driver init."); + return 1; + } + + /* keep waiting for state change for 10 s */ + } while (!status.flag_system_armed); + + /* regular boot, no in-air restart, init IO */ + } else { + + + /* dis-arm IO before touching anything */ + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_ARM_OK | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); + + /* publish RC config to IO */ + ret = io_set_rc_config(); + if (ret != OK) { + log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); + return ret; + } + + } + + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ + ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default PWM output device"); + _primary_pwm_device = true; + } + + /* start the IO interface task */ + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + + return OK; +} + +void +PX4IO::task_main_trampoline(int argc, char *argv[]) +{ + g_dev->task_main(); +} + +void +PX4IO::task_main() +{ + hrt_abstime last_poll_time = 0; + + log("starting"); + + /* + * 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)); + orb_set_interval(_t_actuators, 20); /* default to 50Hz */ + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + + _t_param = orb_subscribe(ORB_ID(parameter_update)); + orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + + /* poll descriptor */ + pollfd fds[4]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + fds[2].fd = _t_vstatus; + fds[2].events = POLLIN; + fds[3].fd = _t_param; + fds[3].events = POLLIN; + + debug("ready"); + + /* lock against the ioctl handler */ + lock(); + + /* loop talking to IO */ + while (!_task_should_exit) { + + /* adjust update interval */ + if (_update_interval != 0) { + if (_update_interval < 5) + _update_interval = 5; + if (_update_interval > 100) + _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); + _update_interval = 0; + } + + /* sleep waiting for topic updates, but no more than 20ms */ + unlock(); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + lock(); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + continue; + } + + perf_begin(_perf_update); + hrt_abstime now = hrt_absolute_time(); + + /* if we have new control data from the ORB, handle it */ + if (fds[0].revents & POLLIN) + io_set_control_state(); + + /* if we have an arming state update, handle it */ + if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) + io_set_arming_state(); + + /* + * If it's time for another tick of the polling status machine, + * try it now. + */ + if ((now - last_poll_time) >= 20000) { + + /* + * Pull status and alarms from IO. + */ + io_get_status(); + + /* + * Get raw R/C input from IO. + */ + io_publish_raw_rc(); + + /* + * Fetch mixed servo controls and PWM outputs from IO. + * + * XXX We could do this at a reduced rate in many/most cases. + */ + io_publish_mixed_controls(); + io_publish_pwm_outputs(); + + /* + * If parameters have changed, re-send RC mappings to IO + * + * XXX this may be a bit spammy + */ + if (fds[3].revents & POLLIN) { + parameter_update_s pupdate; + + /* copy to reset the notification */ + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + + /* re-upload RC input config as it may have changed */ + io_set_rc_config(); + } + } + + perf_end(_perf_update); + } + + unlock(); + + debug("exiting"); + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +PX4IO::io_set_control_state() +{ + actuator_controls_s controls; ///< actuator outputs + uint16_t regs[_max_actuators]; + + /* get controls */ + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &controls); + + for (unsigned i = 0; i < _max_controls; i++) + regs[i] = FLOAT_TO_REG(controls.control[i]); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); +} + +int +PX4IO::io_set_arming_state() +{ + actuator_armed_s armed; ///< system armed state + vehicle_status_s vstatus; ///< overall system state + + orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + + uint16_t set = 0; + uint16_t clear = 0; + + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_ARM_OK; + } + if (vstatus.flag_vector_flight_mode_ok) { + set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + } + if (vstatus.flag_external_manual_override_ok) { + set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } + + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); +} + +int +PX4IO::io_set_rc_config() +{ + unsigned offset = 0; + int input_map[_max_rc_input]; + int32_t ichan; + int ret = OK; + + /* + * Generate the input channel -> control channel mapping table; + * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical + * controls. + */ + for (unsigned i = 0; i < _max_rc_input; i++) + input_map[i] = -1; + + /* + * NOTE: The indices for mapped channels are 1-based + * for compatibility reasons with existing + * autopilots / GCS'. + */ + param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 0; + + param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 1; + + param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 2; + + param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 3; + + ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) + if (input_map[i] == -1) + input_map[i] = ichan++; + + /* + * Iterate all possible RC inputs. + */ + for (unsigned i = 0; i < _max_rc_input; i++) { + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + char pname[16]; + float fval; + + /* + * RC params are floats, but do only + * contain integer values. Do not scale + * or cast them, let the auto-typeconversion + * do its job here. + * Channels: 500 - 2500 + * Inverted flag: -1 (inverted) or 1 (normal) + */ + + sprintf(pname, "RC%d_MIN", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MIN] = fval; + + sprintf(pname, "RC%d_TRIM", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_CENTER] = fval; + + sprintf(pname, "RC%d_MAX", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_MAX] = fval; + + sprintf(pname, "RC%d_DZ", i + 1); + param_get(param_find(pname), &fval); + regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval; + + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i]; + + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + sprintf(pname, "RC%d_REV", i + 1); + param_get(param_find(pname), &fval); + + /* + * This has been taken for the sake of compatibility + * with APM's setup / mission planner: normal: 1, + * inverted: -1 + */ + if (fval < 0) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + + /* send channel config to IO */ + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) { + log("rc config upload failed"); + break; + } + + /* check the IO initialisation flag */ + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + log("config for RC%d rejected by IO", i + 1); + break; + } + + offset += PX4IO_P_RC_CONFIG_STRIDE; + } + + return ret; +} + +int +PX4IO::io_handle_status(uint16_t status) +{ + int ret = 1; + /** + * WARNING: This section handles in-air resets. + */ + + /* check for IO reset - force it back to armed if necessary */ + if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + /* set the arming flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + + /* set new status */ + _status = status; + _status &= PX4IO_P_STATUS_FLAGS_ARMED; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + /* set the sync flag */ + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC); + /* set new status */ + _status = status; + + } else { + ret = 0; + + /* set new status */ + _status = status; + } + + return ret; +} + +int +PX4IO::io_handle_alarms(uint16_t alarms) +{ + + /* XXX handle alarms */ + + + /* set new alarms state */ + _alarms = alarms; + + return 0; +} + +int +PX4IO::io_get_status() +{ + uint16_t regs[4]; + int ret; + + /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + if (ret != OK) + return ret; + + io_handle_status(regs[0]); + io_handle_alarms(regs[1]); + + /* only publish if battery has a valid minimum voltage */ + if (regs[2] > 3300) { + battery_status_s battery_status; + + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = regs[2] / 1000.0f; + + /* current scaling should be to cA in order to avoid limiting at 65A */ + battery_status.current_a = regs[3] / 100.f; + + /* this requires integration over time - not currently implemented */ + battery_status.discharged_mah = -1.0f; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } + } + return ret; +} + +int +PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) +{ + uint32_t channel_count; + int ret = OK; + + /* we don't have the status bits, so input_source has to be set elsewhere */ + input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; + + /* + * XXX Because the channel count and channel data are fetched + * separately, there is a risk of a race between the two + * that could leave us with channel data and a count that + * are out of sync. + * Fixing this would require a guarantee of atomicity from + * IO, and a single fetch for both count and channels. + * + * XXX Since IO has the input calibration info, we ought to be + * able to get the pre-fixed-up controls directly. + * + * XXX can we do this more cheaply? If we knew we had DMA, it would + * almost certainly be better to just get all the inputs... + */ + channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + if (channel_count == _io_reg_get_error) + return -EIO; + if (channel_count > RC_INPUT_MAX_CHANNELS) + channel_count = RC_INPUT_MAX_CHANNELS; + input_rc.channel_count = channel_count; + + if (channel_count > 0) { + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count); + if (ret == OK) + input_rc.timestamp = hrt_absolute_time(); + } + + return ret; +} + +int +PX4IO::io_publish_raw_rc() +{ + /* if no raw RC, just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + return OK; + + /* fetch values from IO */ + rc_input_values rc_val; + rc_val.timestamp = hrt_absolute_time(); + + int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) + return ret; + + /* sort out the source of the values */ + if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* lazily advertise on first publication */ + if (_to_input_rc == 0) { + _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); + } else { + orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); + } + + return OK; +} + +int +PX4IO::io_publish_mixed_controls() +{ + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; + + /* if not taking raw PPM from us, must be mixing */ + if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM) + return OK; + + /* data we are going to fetch */ + actuator_controls_effective_s controls_effective; + controls_effective.timestamp = hrt_absolute_time(); + + /* get actuator controls from IO */ + uint16_t act[_max_actuators]; + int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) + return ret; + + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + + /* laxily advertise on first publication */ + if (_to_actuators_effective == 0) { + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); + } + + return OK; +} + +int +PX4IO::io_publish_pwm_outputs() +{ + /* if no FMU comms(!) just don't publish */ + if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) + return OK; + + /* data we are going to fetch */ + actuator_outputs_s outputs; + outputs.timestamp = hrt_absolute_time(); + + /* get servo values from IO */ + uint16_t ctl[_max_actuators]; + int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) + return ret; + + /* convert from register format to float */ + for (unsigned i = 0; i < _max_actuators; i++) + outputs.output[i] = ctl[i]; + outputs.noutputs = _max_actuators; + + /* lazily advertise on first publication */ + if (_to_outputs == 0) { + _to_outputs = 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)), + _to_outputs, + &outputs); + } + + return OK; +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) +{ + /* range check the transfer */ + if (num_values > ((_max_transfer) / sizeof(*values))) { + debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2); + return -EINVAL; + } + + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_NORESTART; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + /* perform the transfer */ + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_set: error %d", ret); + return ret; +} + +int +PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value) +{ + return io_reg_set(page, offset, &value, 1); +} + +int +PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values) +{ + /* set up the transfer */ + uint8_t addr[2] = { + page, + offset + }; + i2c_msg_s msgv[2]; + + msgv[0].flags = 0; + msgv[0].buffer = addr; + msgv[0].length = 2; + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = (uint8_t *)values; + msgv[1].length = num_values * sizeof(*values); + + /* perform the transfer */ + int ret = transfer(msgv, 2); + if (ret != OK) + debug("io_reg_get: data error %d", ret); + return ret; +} + +uint32_t +PX4IO::io_reg_get(uint8_t page, uint8_t offset) +{ + uint16_t value; + + if (io_reg_get(page, offset, &value, 1)) + return _io_reg_get_error; + + return value; +} + +int +PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits) +{ + int ret; + uint16_t value; + + ret = io_reg_get(page, offset, &value, 1); + if (ret) + return ret; + value &= ~clearbits; + value |= setbits; + + return io_reg_set(page, offset, value); +} + +int +PX4IO::mixer_send(const char *buf, unsigned buflen) +{ + uint8_t frame[_max_transfer]; + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); + + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; + + do { + unsigned count = buflen; + + if (count > max_len) + count = max_len; + + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + } + + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + /* check for the mixer-OK flag */ + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); + return 0; + } else { + debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + } + + /* load must have failed for some reason */ + return -EINVAL; +} + +void +PX4IO::print_status() +{ + /* basic configuration */ + printf("protocol %u software %u bootloader %u buffer %uB\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + + /* status */ + printf("%u bytes free\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", + flags, + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + printf("alarms 0x%04x%s%s%s%s%s%s%s\n", + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); + printf("vbatt %u ibatt %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); + printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); + uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); + printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { + if (mapped_inputs & (1 << i)) + printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); + } + printf("\n"); + uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); + printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); + + /* setup and state */ + printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + printf("rates 0x%04x default %u alt %u relays 0x%04x\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); + printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); +} + +int +PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +{ + int ret = OK; + + /* regular ioctl? */ + switch (cmd) { + case PWM_SERVO_ARM: + /* set the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK); + break; + + case PWM_SERVO_DISARM: + /* clear the 'armed' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); + break; + + case PWM_SERVO_SET_UPDATE_RATE: + /* set the requested alternate rate */ + if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); + } else { + ret = -EINVAL; + } + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: { + + /* blindly clear the PWM update alarm - might be set for some other reason */ + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + break; + } + + case PWM_SERVO_GET_COUNT: + *(unsigned *)arg = _max_actuators; + break; + + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { + + /* TODO: we could go lower for e.g. TurboPWM */ + unsigned channel = cmd - PWM_SERVO_SET(0); + if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { + ret = -EINVAL; + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + } + + break; + } + + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _max_actuators) { + ret = -EINVAL; + } else { + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); + if (value == _io_reg_get_error) { + ret = -EIO; + } else { + *(servo_position_t *)arg = value; + } + } + break; + } + + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + *(uint32_t *)arg = value; + break; + } + + case GPIO_RESET: + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); + break; + + case GPIO_SET: + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + + case GPIO_CLEAR: + arg &= ((1 << _max_relays) - 1); + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + break; + + case GPIO_GET: + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; + break; + + case MIXERIOCGETOUTPUTCOUNT: + *(unsigned *)arg = _max_actuators; + break; + + case MIXERIOCRESET: + ret = 0; /* load always resets */ + break; + + case MIXERIOCLOADBUF: { + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 1024)); + break; + } + + case RC_INPUT_GET: { + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; + + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); + if (ret != OK) + break; + + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } + + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); + break; + } + + case PX4IO_SET_DEBUG: + /* set the debug level */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); + break; + + case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); + } + break; + + default: + /* not a recognized value */ + ret = -ENOTTY; + } + + return ret; +} + +ssize_t +PX4IO::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + + if (count > _max_actuators) + count = _max_actuators; + if (count > 0) { + int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) + return ret; + } + return count * 2; +} + +int +PX4IO::set_update_rate(int rate) +{ + int interval_ms = 1000 / rate; + if (interval_ms < 5) { + interval_ms = 5; + warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + if (interval_ms > 100) { + interval_ms = 100; + warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + _update_interval = interval_ms; + return 0; +} + + +extern "C" __EXPORT int px4io_main(int argc, char *argv[]); + +namespace +{ + +void +start(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->init()) { + delete g_dev; + errx(1, "driver init failed"); + } + + exit(0); +} + +void +test(void) +{ + int fd; + unsigned servo_count = 0; + unsigned pwm_value = 1000; + int direction = 1; + int ret; + + fd = open("/dev/px4io", O_WRONLY); + + if (fd < 0) + err(1, "failed to open device"); + + if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) + err(1, "failed to get servo count"); + + if (ioctl(fd, PWM_SERVO_ARM, 0)) + err(1, "failed to arm servos"); + + for (;;) { + + /* sweep all servos between 1000..2000 */ + servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) + servos[i] = pwm_value; + + ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) + err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); + + if (direction > 0) { + if (pwm_value < 2000) { + pwm_value++; + } else { + direction = -1; + } + } else { + if (pwm_value > 1000) { + pwm_value--; + } else { + direction = 1; + } + } + + /* readback servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t value; + + if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) + err(1, "error reading PWM servo %d", i); + if (value != servos[i]) + errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); + } + } +} + +void +monitor(void) +{ + unsigned cancels = 3; + printf("Hit <enter> three times to exit monitor mode\n"); + + for (;;) { + pollfd fds[1]; + + fds[0].fd = 0; + fds[0].events = POLLIN; + poll(fds, 1, 500); + + if (fds[0].revents == POLLIN) { + int c; + read(0, &c, 1); + + if (cancels-- == 0) + exit(0); + } + +#warning implement this + +// if (g_dev != nullptr) +// g_dev->dump_one = true; + } +} + +} + +int +px4io_main(int argc, char *argv[]) +{ + /* check for sufficient number of arguments */ + if (argc < 2) + goto out; + + if (!strcmp(argv[1], "start")) + start(argc - 1, argv + 1); + + if (!strcmp(argv[1], "limit")) { + + if (g_dev != nullptr) { + + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); + } else { + errx(1, "missing argument (50 - 200 Hz)"); + return 1; + } + } + exit(0); + } + + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); + } + exit(0); + } + + + if (!strcmp(argv[1], "status")) { + + if (g_dev != nullptr) { + printf("[px4io] loaded\n"); + g_dev->print_status(); + } else { + printf("[px4io] not loaded\n"); + } + + exit(0); + } + + if (!strcmp(argv[1], "debug")) { + if (argc <= 2) { + printf("usage: px4io debug LEVEL\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + uint8_t level = atoi(argv[2]); + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); + if (ret != 0) { + printf("SET_DEBUG failed - %d\n", ret); + exit(1); + } + printf("SET_DEBUG %u OK\n", (unsigned)level); + exit(0); + } + + /* note, stop not currently implemented */ + + if (!strcmp(argv[1], "update")) { + + if (g_dev != nullptr) { + printf("[px4io] loaded, detaching first\n"); + /* stop the driver */ + delete g_dev; + } + + PX4IO_Uploader *up; + const char *fn[3]; + + /* work out what we're uploading... */ + if (argc > 2) { + fn[0] = argv[2]; + fn[1] = nullptr; + + } else { + fn[0] = "/fs/microsd/px4io.bin"; + fn[1] = "/etc/px4io.bin"; + fn[2] = nullptr; + } + + up = new PX4IO_Uploader; + int ret = up->upload(&fn[0]); + delete up; + + switch (ret) { + case OK: + break; + + case -ENOENT: + errx(1, "PX4IO firmware file not found"); + + case -EEXIST: + case -EIO: + errx(1, "error updating PX4IO - check that bootloader mode is enabled"); + + case -EINVAL: + errx(1, "verify failed - retry the update"); + + case -ETIMEDOUT: + errx(1, "timed out waiting for bootloader - power-cycle and try again"); + + default: + errx(1, "unexpected error %d", ret); + } + + return ret; + } + + if (!strcmp(argv[1], "rx_dsm") || + !strcmp(argv[1], "rx_dsm_10bit") || + !strcmp(argv[1], "rx_dsm_11bit") || + !strcmp(argv[1], "rx_sbus") || + !strcmp(argv[1], "rx_ppm")) + errx(0, "receiver type is automatically detected, option '%s' is deprecated", argv[1]); + + if (!strcmp(argv[1], "test")) + test(); + + if (!strcmp(argv[1], "monitor")) + monitor(); + + out: + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'"); +} diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp new file mode 100644 index 000000000..9a67875e8 --- /dev/null +++ b/src/drivers/px4io/uploader.cpp @@ -0,0 +1,604 @@ +/**************************************************************************** + * + * 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 uploader.cpp + * Firmware uploader for PX4IO + */ + +#include <nuttx/config.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdbool.h> +#include <assert.h> +#include <errno.h> +#include <string.h> +#include <stdio.h> +#include <stdarg.h> +#include <unistd.h> +#include <fcntl.h> +#include <poll.h> +#include <sys/stat.h> + +#include "uploader.h" + +static const uint32_t crctab[] = +{ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d +}; + +static uint32_t +crc32(const uint8_t *src, unsigned len, unsigned state) +{ + for (unsigned i = 0; i < len; i++) + state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8); + return state; +} + +PX4IO_Uploader::PX4IO_Uploader() : + _io_fd(-1), + _fw_fd(-1) +{ +} + +PX4IO_Uploader::~PX4IO_Uploader() +{ +} + +int +PX4IO_Uploader::upload(const char *filenames[]) +{ + int ret; + const char *filename = NULL; + size_t fw_size; + + _io_fd = open("/dev/ttyS2", O_RDWR); + + if (_io_fd < 0) { + log("could not open interface"); + return -errno; + } + + /* look for the bootloader */ + ret = sync(); + + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + + for (unsigned i = 0; filenames[i] != nullptr; i++) { + _fw_fd = open(filenames[i], O_RDONLY); + + if (_fw_fd < 0) { + log("failed to open %s", filenames[i]); + continue; + } + + log("using firmware from %s", filenames[i]); + filename = filenames[i]; + break; + } + + if (filename == NULL) { + log("no firmware found"); + return -ENOENT; + } + + struct stat st; + if (stat(filename, &st) != 0) { + log("Failed to stat %s - %d\n", filename, (int)errno); + return -errno; + } + fw_size = st.st_size; + + if (_fw_fd == -1) + return -ENOENT; + + /* do the usual program thing - allow for failure */ + for (unsigned retries = 0; retries < 1; retries++) { + if (retries > 0) { + log("retrying update..."); + ret = sync(); + + if (ret != OK) { + /* this is immediately fatal */ + log("bootloader not responding"); + return -EIO; + } + } + + ret = get_info(INFO_BL_REV, bl_rev); + + if (ret == OK) { + if (bl_rev <= BL_REV) { + log("found bootloader revision: %d", bl_rev); + } else { + log("found unsupported bootloader revision %d, exiting", bl_rev); + return OK; + } + } + + ret = erase(); + + if (ret != OK) { + log("erase failed"); + continue; + } + + ret = program(fw_size); + + if (ret != OK) { + log("program failed"); + continue; + } + + if (bl_rev <= 2) + ret = verify_rev2(fw_size); + else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } + + if (ret != OK) { + log("verify failed"); + continue; + } + + ret = reboot(); + + if (ret != OK) { + log("reboot failed"); + return ret; + } + + log("update complete"); + + ret = OK; + break; + } + + close(_fw_fd); + return ret; +} + +int +PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) +{ + struct pollfd fds[1]; + + fds[0].fd = _io_fd; + fds[0].events = POLLIN; + + /* wait 100 ms for a character */ + int ret = ::poll(&fds[0], 1, timeout); + + if (ret < 1) { + log("poll timeout %d", ret); + return -ETIMEDOUT; + } + + read(_io_fd, &c, 1); + //log("recv 0x%02x", c); + return OK; +} + +int +PX4IO_Uploader::recv(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = recv(*p++, 5000); + + if (ret != OK) + return ret; + } + + return OK; +} + +void +PX4IO_Uploader::drain() +{ + uint8_t c; + int ret; + + do { + ret = recv(c, 1000); + + if (ret == OK) { + //log("discard 0x%02x", c); + } + } while (ret == OK); +} + +int +PX4IO_Uploader::send(uint8_t c) +{ + //log("send 0x%02x", c); + if (write(_io_fd, &c, 1) != 1) + return -errno; + + return OK; +} + +int +PX4IO_Uploader::send(uint8_t *p, unsigned count) +{ + while (count--) { + int ret = send(*p++); + + if (ret != OK) + return ret; + } + + return OK; +} + +int +PX4IO_Uploader::get_sync(unsigned timeout) +{ + uint8_t c[2]; + int ret; + + ret = recv(c[0], timeout); + + if (ret != OK) + return ret; + + ret = recv(c[1], timeout); + + if (ret != OK) + return ret; + + if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) { + log("bad sync 0x%02x,0x%02x", c[0], c[1]); + return -EIO; + } + + return OK; +} + +int +PX4IO_Uploader::sync() +{ + drain(); + + /* complete any pending program operation */ + for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) + send(0); + + send(PROTO_GET_SYNC); + send(PROTO_EOC); + return get_sync(); +} + +int +PX4IO_Uploader::get_info(int param, uint32_t &val) +{ + int ret; + + send(PROTO_GET_DEVICE); + send(param); + send(PROTO_EOC); + + ret = recv((uint8_t *)&val, sizeof(val)); + + if (ret != OK) + return ret; + + return get_sync(); +} + +int +PX4IO_Uploader::erase() +{ + log("erase..."); + send(PROTO_CHIP_ERASE); + send(PROTO_EOC); + return get_sync(10000); /* allow 10s timeout */ +} + + +static int read_with_retry(int fd, void *buf, size_t n) +{ + int ret; + uint8_t retries = 0; + do { + ret = read(fd, buf, n); + } while (ret == -1 && retries++ < 100); + if (retries != 0) { + printf("read of %u bytes needed %u retries\n", + (unsigned)n, + (unsigned)retries); + } + return ret; +} + +int +PX4IO_Uploader::program(size_t fw_size) +{ + uint8_t file_buf[PROG_MULTI_MAX]; + ssize_t count; + int ret; + size_t sent = 0; + + log("programming %u bytes...", (unsigned)fw_size); + + ret = lseek(_fw_fd, 0, SEEK_SET); + + while (sent < fw_size) { + /* get more bytes to program */ + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } + + if (count == 0) + return OK; + + sent += count; + + if (count < 0) + return -errno; + + ASSERT((count % 4) == 0); + + send(PROTO_PROG_MULTI); + send(count); + send(&file_buf[0], count); + send(PROTO_EOC); + + ret = get_sync(1000); + + if (ret != OK) + return ret; + } + return OK; +} + +int +PX4IO_Uploader::verify_rev2(size_t fw_size) +{ + uint8_t file_buf[4]; + ssize_t count; + int ret; + size_t sent = 0; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + send(PROTO_CHIP_VERIFY); + send(PROTO_EOC); + ret = get_sync(); + + if (ret != OK) + return ret; + + while (sent < fw_size) { + /* get more bytes to verify */ + size_t n = fw_size - sent; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)sent, + (int)count, + (int)errno); + } + + if (count == 0) + break; + + sent += count; + + if (count < 0) + return -errno; + + ASSERT((count % 4) == 0); + + send(PROTO_READ_MULTI); + send(count); + send(PROTO_EOC); + + for (ssize_t i = 0; i < count; i++) { + uint8_t c; + + ret = recv(c, 5000); + + if (ret != OK) { + log("%d: got %d waiting for bytes", sent + i, ret); + return ret; + } + + if (c != file_buf[i]) { + log("%d: got 0x%02x expected 0x%02x", sent + i, c, file_buf[i]); + return -EINVAL; + } + } + + ret = get_sync(); + + if (ret != OK) { + log("timeout waiting for post-verify sync"); + return ret; + } + } + + return OK; +} + +int +PX4IO_Uploader::verify_rev3(size_t fw_size_local) +{ + int ret; + uint8_t file_buf[4]; + ssize_t count; + uint32_t sum = 0; + uint32_t bytes_read = 0; + uint32_t crc = 0; + uint32_t fw_size_remote; + uint8_t fill_blank = 0xff; + + log("verify..."); + lseek(_fw_fd, 0, SEEK_SET); + + ret = get_info(INFO_FLASH_SIZE, fw_size_remote); + send(PROTO_EOC); + + if (ret != OK) { + log("could not read firmware size"); + return ret; + } + + /* read through the firmware file again and calculate the checksum*/ + while (bytes_read < fw_size_local) { + size_t n = fw_size_local - bytes_read; + if (n > sizeof(file_buf)) { + n = sizeof(file_buf); + } + count = read_with_retry(_fw_fd, file_buf, n); + + if (count != (ssize_t)n) { + log("firmware read of %u bytes at %u failed -> %d errno %d", + (unsigned)n, + (unsigned)bytes_read, + (int)count, + (int)errno); + } + + /* set the rest to ff */ + if (count == 0) { + break; + } + /* stop if the file cannot be read */ + if (count < 0) + return -errno; + + /* calculate crc32 sum */ + sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum); + + bytes_read += count; + } + + /* fill the rest with 0xff */ + while (bytes_read < fw_size_remote) { + sum = crc32(&fill_blank, sizeof(fill_blank), sum); + bytes_read += sizeof(fill_blank); + } + + /* request CRC from IO */ + send(PROTO_GET_CRC); + send(PROTO_EOC); + + ret = recv((uint8_t*)(&crc), sizeof(crc)); + + if (ret != OK) { + log("did not receive CRC checksum"); + return ret; + } + + /* compare the CRC sum from the IO with the one calculated */ + if (sum != crc) { + log("CRC wrong: received: %d, expected: %d", crc, sum); + return -EINVAL; + } + + return OK; +} + +int +PX4IO_Uploader::reboot() +{ + send(PROTO_REBOOT); + send(PROTO_EOC); + + return OK; +} + +void +PX4IO_Uploader::log(const char *fmt, ...) +{ + va_list ap; + + printf("[PX4IO] "); + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); + printf("\n"); + fflush(stdout); +} diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h new file mode 100644 index 000000000..135202dd1 --- /dev/null +++ b/src/drivers/px4io/uploader.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * 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 uploader.h + * Firmware uploader definitions for PX4IO. + */ + +#ifndef _PX4IO_UPLOADER_H +#define _PX4IO_UPLOADER_H value + +#include <stdint.h> +#include <stdbool.h> + + +class PX4IO_Uploader +{ +public: + PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); + + int upload(const char *filenames[]); + +private: + enum { + + PROTO_NOP = 0x00, + PROTO_OK = 0x10, + PROTO_FAILED = 0x11, + PROTO_INSYNC = 0x12, + PROTO_EOC = 0x20, + PROTO_GET_SYNC = 0x21, + PROTO_GET_DEVICE = 0x22, + PROTO_CHIP_ERASE = 0x23, + PROTO_CHIP_VERIFY = 0x24, + PROTO_PROG_MULTI = 0x27, + PROTO_READ_MULTI = 0x28, + PROTO_GET_CRC = 0x29, + PROTO_REBOOT = 0x30, + + INFO_BL_REV = 1, /**< bootloader protocol revision */ + BL_REV = 3, /**< supported bootloader protocol */ + INFO_BOARD_ID = 2, /**< board type */ + INFO_BOARD_REV = 3, /**< board revision */ + INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */ + + PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */ + READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */ + + }; + + int _io_fd; + int _fw_fd; + + uint32_t bl_rev; /**< bootloader revision */ + + void log(const char *fmt, ...); + + int recv(uint8_t &c, unsigned timeout); + int recv(uint8_t *p, unsigned count); + void drain(); + int send(uint8_t c); + int send(uint8_t *p, unsigned count); + int get_sync(unsigned timeout = 1000); + int sync(); + int get_info(int param, uint32_t &val); + int erase(); + int program(size_t fw_size); + int verify_rev2(size_t fw_size); + int verify_rev3(size_t fw_size); + int reboot(); +}; + +#endif diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1a50dde0f..8e18c3c9a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -46,7 +46,6 @@ #include <stdbool.h> #include <poll.h> #include <fcntl.h> -#include <v1.0/common/mavlink.h> #include <float.h> #include <nuttx/sched.h> #include <sys/prctl.h> diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index bfb7db997..d98647f99 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -35,10 +35,10 @@ # Attitude estimator (Extended Kalman Filter) # -MODULE_NAME = attitude_estimator_ekf -CXXSRCS = attitude_estimator_ekf_main.cpp +MODULE_COMMAND = attitude_estimator_ekf -SRCS = attitude_estimator_ekf_params.c \ +SRCS = attitude_estimator_ekf_main.cpp \ + attitude_estimator_ekf_params.c \ codegen/eye.c \ codegen/attitudeKalmanfilter.c \ codegen/mrdivide.c \ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index b90da8289..556d5c2df 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c new file mode 100755 index 000000000..977565b8e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c @@ -0,0 +1,58 @@ +/* + * 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 new file mode 100755 index 000000000..2f5330563 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h @@ -0,0 +1,30 @@ +/* + * 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 <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..6627f76e9 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..a77eb5712 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h @@ -0,0 +1,30 @@ +/* + * 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 <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..a65536f79 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..100c7f76c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h @@ -0,0 +1,30 @@ +/* + * 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 <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..d4b2c2d61 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..11b999064 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c @@ -0,0 +1,119 @@ +/* + * 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 new file mode 100755 index 000000000..30170ae22 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h @@ -0,0 +1,32 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> +#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 new file mode 100755 index 000000000..de5a1d8aa --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..3d507ff31 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h @@ -0,0 +1,32 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> +#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 new file mode 100755 index 000000000..0757c878c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..23995020b --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h @@ -0,0 +1,32 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> +#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 new file mode 100755 index 000000000..f7a04d908 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..9efe2ea7a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c @@ -0,0 +1,137 @@ +/*
+ * 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 new file mode 100755 index 000000000..9bbffbbb3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h @@ -0,0 +1,33 @@ +/*
+ * 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 <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 new file mode 100755 index 000000000..8f2275c13 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c @@ -0,0 +1,32 @@ +/*
+ * 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 new file mode 100755 index 000000000..952eb7b89 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h @@ -0,0 +1,38 @@ +/*
+ * 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 <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 new file mode 100755 index 000000000..b87d604c4 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c @@ -0,0 +1,47 @@ +/*
+ * 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 new file mode 100755 index 000000000..9dee90f9e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h @@ -0,0 +1,33 @@ +/*
+ * 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 <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 new file mode 100755 index 000000000..b00858205 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c @@ -0,0 +1,31 @@ +/*
+ * 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 new file mode 100755 index 000000000..69cc85c76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h @@ -0,0 +1,33 @@ +/*
+ * 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 <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 new file mode 100755 index 000000000..f36ea4557 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h @@ -0,0 +1,16 @@ +/*
+ * 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 new file mode 100755 index 000000000..5139848bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c @@ -0,0 +1,136 @@ +/* + * 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 new file mode 100755 index 000000000..205c8eb4e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..4c535618a --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c @@ -0,0 +1,157 @@ +/* + * 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 new file mode 100755 index 000000000..94cbe2ce6 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..aa89f8a9d --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..8d358a9a3 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..20ed2edbb --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..5eb5807a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..43e5f016c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..5bd87c390 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..44bce472f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..41e11936f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c @@ -0,0 +1,31 @@ +/* + * 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 new file mode 100755 index 000000000..e84ea01bc --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h @@ -0,0 +1,31 @@ +/* + * 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 <math.h> +#include <stddef.h> +#include <stdlib.h> + +#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 new file mode 100755 index 000000000..4b473f56f --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h @@ -0,0 +1,16 @@ +/* + * 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 new file mode 100755 index 000000000..51aef7b76 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.c @@ -0,0 +1,524 @@ +/*
+ * 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;
+
+ /* <LEGAL> This is a uniform (0,1) pseudorandom number generator based on: */
+ /* <LEGAL> */
+ /* <LEGAL> A C-program for MT19937, with initialization improved 2002/1/26. */
+ /* <LEGAL> Coded by Takuji Nishimura and Makoto Matsumoto. */
+ /* <LEGAL> */
+ /* <LEGAL> Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */
+ /* <LEGAL> All rights reserved. */
+ /* <LEGAL> */
+ /* <LEGAL> Redistribution and use in source and binary forms, with or without */
+ /* <LEGAL> modification, are permitted provided that the following conditions */
+ /* <LEGAL> are met: */
+ /* <LEGAL> */
+ /* <LEGAL> 1. Redistributions of source code must retain the above copyright */
+ /* <LEGAL> notice, this list of conditions and the following disclaimer. */
+ /* <LEGAL> */
+ /* <LEGAL> 2. Redistributions in binary form must reproduce the above copyright */
+ /* <LEGAL> notice, this list of conditions and the following disclaimer in the */
+ /* <LEGAL> documentation and/or other materials provided with the distribution. */
+ /* <LEGAL> */
+ /* <LEGAL> 3. The names of its contributors may not be used to endorse or promote */
+ /* <LEGAL> products derived from this software without specific prior written */
+ /* <LEGAL> permission. */
+ /* <LEGAL> */
+ /* <LEGAL> THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */
+ /* <LEGAL> "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */
+ /* <LEGAL> LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */
+ /* <LEGAL> A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR */
+ /* <LEGAL> CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */
+ /* <LEGAL> EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */
+ /* <LEGAL> PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */
+ /* <LEGAL> PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */
+ /* <LEGAL> LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */
+ /* <LEGAL> NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */
+ /* <LEGAL> 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 new file mode 100755 index 000000000..8a2aa9277 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/randn.h @@ -0,0 +1,33 @@ +/*
+ * 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 <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#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 new file mode 100755 index 000000000..c6fa7884e --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.c @@ -0,0 +1,139 @@ +/* + * 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 new file mode 100755 index 000000000..e7b2a2d1c --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetInf.h @@ -0,0 +1,23 @@ +/* + * 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 <stddef.h> +#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 new file mode 100755 index 000000000..552770149 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.c @@ -0,0 +1,96 @@ +/* + * 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 new file mode 100755 index 000000000..5acdd9790 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtGetNaN.h @@ -0,0 +1,21 @@ +/* + * 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 <stddef.h> +#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 new file mode 100755 index 000000000..de121c4a0 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c @@ -0,0 +1,87 @@ +/* + * 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 new file mode 100755 index 000000000..3bbcfd087 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h @@ -0,0 +1,53 @@ +/* + * 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 <float.h> +#endif +#include <stddef.h> +#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 new file mode 100755 index 000000000..8916e8572 --- /dev/null +++ b/src/modules/position_estimator_mc/codegen/rtwtypes.h @@ -0,0 +1,159 @@ +/* + * 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 <limits.h> + +/*=======================================================================* + * 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 new file mode 100755 index 000000000..ff939d029 --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe1.m @@ -0,0 +1,3 @@ +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 new file mode 100755 index 000000000..2a50164ef --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe2.m @@ -0,0 +1,9 @@ +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 new file mode 100755 index 000000000..4c6421b7f --- /dev/null +++ b/src/modules/position_estimator_mc/kalman_dlqe3.m @@ -0,0 +1,17 @@ +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 new file mode 100644 index 000000000..40b135ea4 --- /dev/null +++ b/src/modules/position_estimator_mc/module.mk @@ -0,0 +1,60 @@ +############################################################################ +# +# 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 new file mode 100755 index 000000000..144ff8c7c --- /dev/null +++ b/src/modules/position_estimator_mc/positionKalmanFilter1D.m @@ -0,0 +1,19 @@ +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)<thresh + x_apriori(2)=decay*x_apriori(2); + end + %update + if gps_update==1 + y=z-C*x_apriori; + S=C*P_apriori*C'+R; + K=(P_apriori*C')/S; + x_aposteriori=x_apriori+K*y; + P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; + else + x_aposteriori=x_apriori; + P_aposteriori=P_apriori; + end +end diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m new file mode 100755 index 000000000..f94cce1fb --- /dev/null +++ b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m @@ -0,0 +1,26 @@ +function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay) + %dynamics + A = [1 dT -0.5*dT*dT; + 0 1 -dT; + 0 0 1]; + B = [0.5*dT*dT; dT; 0]; + C = [1 0 0]; + %prediction + x_apriori=A*x_aposteriori_k+B*u; + P_apriori=A*P_aposteriori_k*A'+Q; + if abs(u)<thresh + x_apriori(2)=decay*x_apriori(2); + end + %update + if gps_update==1 + y=z-C*x_apriori; + S=C*P_apriori*C'+R; + K=(P_apriori*C')/S; + x_aposteriori=x_apriori+K*y; + P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori; + else + x_aposteriori=x_apriori; + P_aposteriori=P_apriori; + end +end + diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c new file mode 100755 index 000000000..10dee3f22 --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -0,0 +1,510 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger <daregger@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> +* Lorenz Meier <lm@inf.ethz.ch> + * + * 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 <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <string.h> +#include <nuttx/config.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <mavlink/mavlink_log.h> +#include <poll.h> +#include <systemlib/geo/geo.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <drivers/drv_hrt.h> + +#include "position_estimator_mc_params.h" +//#include <uORB/topics/debug_key_value.h> +#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("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<baro_loop_end) { + p0_Pa += (sensor.baro_pres_mbar*100); + baro_loop_cnt++; + } else { + p0_Pa /= (float)(baro_loop_cnt); + flag_baro_initialized = true; + char *baro_m_start = "barometer initialized with p0 = "; + char p0_char[15]; + sprintf(p0_char, "%8.2f", (double)(p0_Pa/100)); + char *baro_m_end = " mbar"; + char str[80]; + strcpy(str,baro_m_start); + strcat(str,p0_char); + strcat(str,baro_m_end); + mavlink_log_info(mavlink_fd, str); + } + } + if (flag_use_gps) { + /* initialize map projection with the last estimate (not at full rate) */ + if (gps.fix_type > 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]))){ + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + } + } + } /* 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 new file mode 100755 index 000000000..72e5bc73b --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger <daregger@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> +* Lorenz Meier <lm@inf.ethz.ch> + * + * 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 new file mode 100755 index 000000000..c4c95f7b4 --- /dev/null +++ b/src/modules/position_estimator_mc/position_estimator_mc_params.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger <daregger@student.ethz.ch> + * Tobias Naegeli <naegelit@student.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * 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 <systemlib/param/param.h> + +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 new file mode 100644 index 000000000..89da2d827 --- /dev/null +++ b/src/modules/sdlog/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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 new file mode 100644 index 000000000..df8745d9f --- /dev/null +++ b/src/modules/sdlog/sdlog.c @@ -0,0 +1,829 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 <lm@inf.ethz.ch> + * + * Simple SD logger for flight data. Buffers new sensor values and + * does the heavy SD I/O in a low-priority worker thread. + */ + +#include <nuttx/config.h> +#include <sys/types.h> +#include <sys/stat.h> +#include <sys/prctl.h> +#include <fcntl.h> +#include <errno.h> +#include <unistd.h> +#include <stdio.h> +#include <poll.h> +#include <stdlib.h> +#include <string.h> +#include <ctype.h> +#include <systemlib/err.h> +#include <unistd.h> +#include <drivers/drv_hrt.h> + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/differential_pressure.h> + +#include <systemlib/systemlib.h> + +#include <mavlink/mavlink_log.h> + +#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 <number of skipped lines>]\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(). + */ +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("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_pressure; + } 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_pressure_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_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + fds[fdsc_count].fd = subs.diff_pressure_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_pressure_sub, &buf.diff_pressure); + 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_pressure.differential_pressure_mbar, + .ind_airspeed = buf.diff_pressure.indicated_airspeed_m_s, + .true_airspeed = buf.diff_pressure.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 new file mode 100644 index 000000000..d7c8a4759 --- /dev/null +++ b/src/modules/sdlog/sdlog_ringbuffer.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 <lm@inf.ethz.ch> + */ + +#include <string.h> +#include <stdlib.h> + +#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 new file mode 100644 index 000000000..b65916459 --- /dev/null +++ b/src/modules/sdlog/sdlog_ringbuffer.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 <lm@inf.ethz.ch> + */ + +#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 |