diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 05a6292a29..b25e612297 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -204,6 +205,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); + struct vehicle_status_s state; + memset(&state, 0, sizeof(state)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -216,6 +219,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); + /* subscribe to system state*/ + int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -226,13 +232,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) thread_running = true; /* advertise debug value */ - struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = -1; + // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; + // orb_advert_t pub_dbg = -1; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; struct attitude_estimator_ekf_params ekf_params; @@ -259,8 +267,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (ret < 0) { /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_status), sub_state, &state); + if (!state.flag_hil_enabled) { + fprintf(stderr, + "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + } } else { /* only update parameters if they changed */ @@ -347,9 +359,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - int32_t z_k_sizes = 9; - // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; - static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ @@ -392,7 +401,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_diff = hrt_absolute_time() - timing_start; + // uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ if (printcounter % 200 == 0) { diff --git a/apps/drivers/hil/Makefile b/apps/drivers/hil/Makefile new file mode 100644 index 0000000000..1fb6e37bce --- /dev/null +++ b/apps/drivers/hil/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 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 PX4FMU board +# + +APPNAME = hil +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp new file mode 100644 index 0000000000..bef21848b5 --- /dev/null +++ b/apps/drivers/hil/hil.cpp @@ -0,0 +1,853 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 hil.cpp + * + * Driver/configurator for the virtual HIL port. + * + * This virtual driver emulates PWM / servo outputs for setups where + * the connected hardware does not provide enough or no PWM outputs. + * + * Its only function is to take actuator_control uORB messages, + * mix them with any loaded mixer and output the result to the + * actuator_output uORB topic. HIL can also be performed with normal + * PWM outputs, a special flag prevents the outputs to be operated + * during HIL mode. If HIL is not performed with a standalone FMU, + * but in a real system, it is NOT recommended to use this virtual + * driver. Use instead the normal FMU or IO driver. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +// #include + +#include +#include +#include + +#include +#include + +#include + +class HIL : public device::CDev +{ +public: + enum Mode { + MODE_2PWM, + MODE_4PWM, + MODE_8PWM, + MODE_12PWM, + MODE_16PWM, + MODE_NONE + }; + HIL(); + ~HIL(); + + virtual int ioctl(file *filp, int cmd, unsigned long arg); + + virtual int init(); + + int set_mode(Mode mode); + int set_pwm_rate(unsigned rate); + +private: + static const unsigned _max_actuators = 4; + + Mode _mode; + int _update_rate; + int _current_update_rate; + int _task; + int _t_actuators; + int _t_armed; + orb_advert_t _t_outputs; + unsigned _num_outputs; + bool _primary_pwm_device; + + volatile bool _task_should_exit; + bool _armed; + + MixerGroup *_mixers; + + actuator_controls_s _controls; + + static void task_main_trampoline(int argc, char *argv[]); + void task_main() __attribute__((noreturn)); + + static int control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input); + + int pwm_ioctl(file *filp, int cmd, unsigned long arg); + + struct GPIOConfig { + uint32_t input; + uint32_t output; + uint32_t alt; + }; + + static const GPIOConfig _gpio_tab[]; + static const unsigned _ngpio; + + void gpio_reset(void); + void gpio_set_function(uint32_t gpios, int function); + void gpio_write(uint32_t gpios, int function); + uint32_t gpio_read(void); + int gpio_ioctl(file *filp, int cmd, unsigned long arg); + +}; + +namespace +{ + +HIL *g_hil; + +} // namespace + +HIL::HIL() : + CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), + _mode(MODE_NONE), + _update_rate(50), + _task(-1), + _t_actuators(-1), + _t_armed(-1), + _t_outputs(0), + _num_outputs(0), + _primary_pwm_device(false), + _task_should_exit(false), + _armed(false), + _mixers(nullptr) +{ + _debug_enabled = true; +} + +HIL::~HIL() +{ + if (_task != -1) { + /* tell the task we want it to go away */ + _task_should_exit = true; + + unsigned i = 10; + do { + /* wait 50ms - it should wake every 100ms or so worst-case */ + usleep(50000); + + /* if we have given up, kill it */ + if (--i == 0) { + task_delete(_task); + break; + } + + } while (_task != -1); + } + + /* clean up the alternate device node */ + if (_primary_pwm_device) + unregister_driver(PWM_OUTPUT_DEVICE_PATH); + + g_hil = nullptr; +} + +int +HIL::init() +{ + int ret; + + ASSERT(_task == -1); + + /* do regular cdev init */ + ret = CDev::init(); + + if (ret != OK) + return ret; + + // XXX already claimed with CDEV + ///* 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; + } + + /* reset GPIOs */ + // gpio_reset(); + + /* start the HIL interface task */ + _task = task_spawn("fmuhil", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&HIL::task_main_trampoline, + nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + + return OK; +} + +void +HIL::task_main_trampoline(int argc, char *argv[]) +{ + g_hil->task_main(); +} + +int +HIL::set_mode(Mode mode) +{ + /* + * Configure for PWM output. + * + * Note that regardless of the configured mode, the task is always + * listening and mixing; the mode just selects which of the channels + * are presented on the output pins. + */ + switch (mode) { + case MODE_2PWM: + debug("MODE_2PWM"); + /* multi-port with flow control lines as PWM */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_4PWM: + debug("MODE_4PWM"); + /* multi-port as 4 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_8PWM: + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + break; + + case MODE_NONE: + debug("MODE_NONE"); + /* disable servo outputs and set a very low update rate */ + _update_rate = 10; + break; + + default: + return -EINVAL; + } + _mode = mode; + return OK; +} + +int +HIL::set_pwm_rate(unsigned rate) +{ + if ((rate > 500) || (rate < 10)) + return -EINVAL; + + _update_rate = rate; + return OK; +} + +void +HIL::task_main() +{ + /* + * Subscribe to the appropriate PWM output topic based on whether we are the + * primary PWM output or not. + */ + _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1)); + /* force a reset of the update rate */ + _current_update_rate = 0; + + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + + /* advertise the mixed control outputs */ + actuator_outputs_s outputs; + memset(&outputs, 0, sizeof(outputs)); + /* advertise the mixed control outputs */ + _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), + &outputs); + + pollfd fds[2]; + fds[0].fd = _t_actuators; + fds[0].events = POLLIN; + fds[1].fd = _t_armed; + fds[1].events = POLLIN; + + unsigned num_outputs; + + /* select the number of virtual outputs */ + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + // XXX only support the lower 8 - trivial to extend + num_outputs = 8; + break; + + case MODE_NONE: + default: + num_outputs = 0; + break; + } + + log("starting"); + + /* loop until killed */ + while (!_task_should_exit) { + + /* handle update rate changes */ + if (_current_update_rate != _update_rate) { + int update_rate_in_ms = int(1000 / _update_rate); + if (update_rate_in_ms < 2) + update_rate_in_ms = 2; + orb_set_interval(_t_actuators, update_rate_in_ms); + // up_pwm_servo_set_rate(_update_rate); + _current_update_rate = _update_rate; + } + + /* sleep waiting for data, but no more than a second */ + int ret = ::poll(&fds[0], 2, 1000); + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + usleep(1000000); + continue; + } + + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + /* do mixing */ + _mixers->mix(&outputs.output[0], num_outputs); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + /* output to the servo */ + // up_pwm_servo_set(i, outputs.output[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + } + } + + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + + /* update PWM servo armed status */ + // up_pwm_servo_arm(aa.armed); + } + } + + ::close(_t_actuators); + ::close(_t_armed); + + /* make sure servos are off */ + // up_pwm_servo_deinit(); + + log("stopping"); + + /* note - someone else is responsible for restoring the GPIO config */ + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +HIL::control_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &input) +{ + const actuator_controls_s *controls = (actuator_controls_s *)handle; + + input = controls->control[control_index]; + return 0; +} + +int +HIL::ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret; + + debug("ioctl 0x%04x 0x%08x", cmd, arg); + + // /* try it as a GPIO ioctl first */ + // ret = HIL::gpio_ioctl(filp, cmd, arg); + // if (ret != -ENOTTY) + // return ret; + + /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + switch(_mode) { + case MODE_2PWM: + case MODE_4PWM: + case MODE_8PWM: + case MODE_12PWM: + case MODE_16PWM: + ret = HIL::pwm_ioctl(filp, cmd, arg); + break; + default: + debug("not in a PWM mode"); + break; + } + + /* if nobody wants it, let CDev have it */ + if (ret == -ENOTTY) + ret = CDev::ioctl(filp, cmd, arg); + + return ret; +} + +int +HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + int channel; + + lock(); + + switch (cmd) { + case PWM_SERVO_ARM: + // up_pwm_servo_arm(true); + break; + + case PWM_SERVO_DISARM: + // up_pwm_servo_arm(false); + break; + + case PWM_SERVO_SET(2): + case PWM_SERVO_SET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_SET(0): + case PWM_SERVO_SET(1): + if (arg < 2100) { + // channel = cmd - PWM_SERVO_SET(0); +// up_pwm_servo_set(channel, arg); XXX + + } else { + ret = -EINVAL; + } + + break; + + case PWM_SERVO_GET(2): + case PWM_SERVO_GET(3): + if (_mode != MODE_4PWM) { + ret = -EINVAL; + break; + } + + /* FALLTHROUGH */ + case PWM_SERVO_GET(0): + case PWM_SERVO_GET(1): { + // channel = cmd - PWM_SERVO_SET(0); + // *(servo_position_t *)arg = up_pwm_servo_get(channel); + break; + } + + case MIXERIOCGETOUTPUTCOUNT: + if (_mode == MODE_4PWM) { + *(unsigned *)arg = 4; + + } else { + *(unsigned *)arg = 2; + } + + break; + + case MIXERIOCRESET: + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + break; + + case MIXERIOCADDSIMPLE: { + mixer_simple_s *mixinfo = (mixer_simple_s *)arg; + + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); + + if (mixer->check()) { + delete mixer; + ret = -EINVAL; + + } else { + if (_mixers == nullptr) + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); + + _mixers->add_mixer(mixer); + } + + break; + } + + case MIXERIOCADDMULTIROTOR: + /* XXX not yet supported */ + ret = -ENOTTY; + break; + + case MIXERIOCLOADFILE: { + const char *path = (const char *)arg; + + if (_mixers != nullptr) { + delete _mixers; + _mixers = nullptr; + } + + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + if (_mixers == nullptr) { + ret = -ENOMEM; + } else { + + debug("loading mixers from %s", path); + ret = _mixers->load_from_file(path); + + if (ret != 0) { + debug("mixer load failed with %d", ret); + delete _mixers; + _mixers = nullptr; + ret = -EINVAL; + } + } + + break; + } + + default: + ret = -ENOTTY; + break; + } + + unlock(); + + return ret; +} + +namespace +{ + +enum PortMode { + PORT_MODE_UNDEFINED = 0, + PORT1_MODE_UNSET, + PORT1_FULL_PWM, + PORT1_PWM_AND_SERIAL, + PORT1_PWM_AND_GPIO, + PORT2_MODE_UNSET, + PORT2_8PWM, + PORT2_12PWM, + PORT2_16PWM, +}; + +PortMode g_port_mode; + +int +hil_new_mode(PortMode new_mode, int update_rate) +{ + // uint32_t gpio_bits; + + +// /* reset to all-inputs */ +// g_hil->ioctl(0, GPIO_RESET, 0); + + // gpio_bits = 0; + + HIL::Mode servo_mode = HIL::MODE_NONE; + + switch (new_mode) { + case PORT_MODE_UNDEFINED: + case PORT1_MODE_UNSET: + case PORT2_MODE_UNSET: + /* nothing more to do here */ + break; + + case PORT1_FULL_PWM: + /* select 4-pin PWM mode */ + servo_mode = HIL::MODE_4PWM; + break; + + case PORT1_PWM_AND_SERIAL: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; +// /* set RX/TX multi-GPIOs to serial mode */ +// gpio_bits = GPIO_MULTI_3 | GPIO_MULTI_4; + break; + + case PORT1_PWM_AND_GPIO: + /* select 2-pin PWM mode */ + servo_mode = HIL::MODE_2PWM; + break; + + case PORT2_8PWM: + /* select 8-pin PWM mode */ + servo_mode = HIL::MODE_8PWM; + break; + + case PORT2_12PWM: + /* select 12-pin PWM mode */ + servo_mode = HIL::MODE_12PWM; + break; + + case PORT2_16PWM: + /* select 16-pin PWM mode */ + servo_mode = HIL::MODE_16PWM; + break; + } + +// /* adjust GPIO config for serial mode(s) */ +// if (gpio_bits != 0) +// g_hil->ioctl(0, GPIO_SET_ALT_1, gpio_bits); + + /* (re)set the PWM output mode */ + g_hil->set_mode(servo_mode); + if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0)) + g_hil->set_pwm_rate(update_rate); + + return OK; +} + +int +hil_start(void) +{ + int ret = OK; + + if (g_hil == nullptr) { + + g_hil = new HIL; + + if (g_hil == nullptr) { + ret = -ENOMEM; + + } else { + ret = g_hil->init(); + + if (ret != OK) { + delete g_hil; + g_hil = nullptr; + } + } + } + + return ret; +} + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + exit(1); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + ioctl(fd, PWM_SERVO_SET(0), 1000); + + close(fd); + + exit(0); +} + +void +fake(int argc, char *argv[]) +{ + if (argc < 5) { + puts("hil fake (values -100 .. 100)"); + exit(1); + } + + actuator_controls_s ac; + + ac.control[0] = strtol(argv[1], 0, 0) / 100.0f; + + ac.control[1] = strtol(argv[2], 0, 0) / 100.0f; + + ac.control[2] = strtol(argv[3], 0, 0) / 100.0f; + + ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; + + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + + if (handle < 0) { + puts("advertise failed"); + exit(1); + } + + exit(0); +} + +} // namespace + +extern "C" __EXPORT int hil_main(int argc, char *argv[]); + +int +hil_main(int argc, char *argv[]) +{ + PortMode new_mode = PORT_MODE_UNDEFINED; + int pwm_update_rate_in_hz = 0; + + if (!strcmp(argv[1], "test")) + test(); + + if (!strcmp(argv[1], "fake")) + fake(argc - 1, argv + 1); + + if (hil_start() != OK) + errx(1, "failed to start the FMU driver"); + + /* + * Mode switches. + * + * XXX use getopt? + */ + for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ + + if (!strcmp(argv[i], "mode_pwm")) { + new_mode = PORT1_FULL_PWM; + + } else if (!strcmp(argv[i], "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; + + } else if (!strcmp(argv[i], "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; + + } else if (!strcmp(argv[i], "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(argv[i], "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; + + } else if (!strcmp(argv[i], "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } + + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { + // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { + // XXX all modes have PWM settings + if (argc > i + 1) { + pwm_update_rate_in_hz = atoi(argv[i + 1]); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + // } else { + // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); + // } + } + } + + /* was a new mode set? */ + if (new_mode != PORT_MODE_UNDEFINED) { + + /* yes but it's the same mode */ + if (new_mode == g_port_mode) + return OK; + + /* switch modes */ + return hil_new_mode(new_mode, pwm_update_rate_in_hz); + } + + /* test, etc. here */ + + fprintf(stderr, "HIL: unrecognized command, try:\n"); + fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + return -EINVAL; +} diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index fff713bb5e..3eb4a9ef22 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -224,7 +224,7 @@ PX4FMU::init() _task = task_spawn("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1024, + 2048, (main_t)&PX4FMU::task_main_trampoline, nullptr); @@ -419,7 +419,8 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - debug("ioctl 0x%04x 0x%08x", cmd, arg); + // XXX disabled, confusing users + //debug("ioctl 0x%04x 0x%08x", cmd, arg); /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 9c39b2714f..2527e0b0ff 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -132,6 +132,7 @@ static struct mavlink_logbuffer lb; static void mavlink_update_system(void); static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); @@ -143,15 +144,10 @@ set_hil_on_off(bool hil_enabled) /* Enable HIL */ if (hil_enabled && !mavlink_hil_enabled) { - //printf("\n HIL ON \n"); - /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - printf("\n pub_hil_attitude :%i\n", pub_hil_attitude); - printf("\n pub_hil_global_pos :%i\n", pub_hil_global_pos); - mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ @@ -166,15 +162,13 @@ set_hil_on_off(bool hil_enabled) } else if (baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; - } else if (baudrate < 460800) { - /* 50 Hz */ - hil_rate_interval = 20; } else { /* 100 Hz */ hil_rate_interval = 10; } orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { @@ -268,7 +262,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } -static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -458,19 +452,19 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) /* * Internal function to give access to the channel status for each channel */ -mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +mavlink_status_t* mavlink_get_channel_status(uint8_t channel) { static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; + return &m_mavlink_status[channel]; } /* * Internal function to give access to the channel buffer for each channel */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) +mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) { static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[chan]; + return &m_mavlink_buffer[channel]; } void mavlink_update_system(void) diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 39e574c047..3e485274eb 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file mavlink_receiver.c * MAVLink protocol message receive and dispatch + * + * @author Lorenz Meier */ /* XXX trim includes */ @@ -216,8 +218,6 @@ handle_message(mavlink_message_t *msg) if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - //printf("got message\n"); - //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); if (mavlink_system.sysid < 4) { @@ -274,61 +274,6 @@ handle_message(mavlink_message_t *msg) /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - - // /* change armed status if required */ - // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - // bool cmd_generated = false; - - // if (v_status.flag_control_offboard_enabled != cmd_armed) { - // vcmd.param1 = cmd_armed; - // vcmd.param2 = 0; - // vcmd.param3 = 0; - // vcmd.param4 = 0; - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // /* check if input has to be enabled */ - // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // if (cmd_generated) { - // /* check if topic is advertised */ - // if (cmd_pub <= 0) { - // cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - // } else { - // /* create command */ - // orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - // } - // } } } @@ -340,8 +285,6 @@ handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); - if (mavlink_hil_enabled) { if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { @@ -349,20 +292,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); - // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " - // "ROLL %i \n PITCH %i \n YAW %i \n" - // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", - // hil_state.lat/1000000, // 1e7 - // hil_state.lon/1000000, // 1e7 - // hil_state.alt/1000, // mm - // hil_state.roll, // float rad - // hil_state.pitch, // float rad - // hil_state.yaw, // float rad - // hil_state.rollspeed, // float rad/s - // hil_state.pitchspeed, // float rad/s - // hil_state.yawspeed); // float rad/s - - hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 683964a0d9..3ac229d734 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +35,8 @@ /** * @file orb_listener.c * Monitors ORB topics and sends update messages as appropriate. + * + * @author Lorenz Meier */ // XXX trim includes @@ -419,7 +421,7 @@ l_actuator_outputs(struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - if (gcs_link) + if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, l->arg /* port number */, act_outputs.output[0], @@ -430,6 +432,90 @@ l_actuator_outputs(struct listener *l) act_outputs.output[5], act_outputs.output[6], act_outputs.output[7]); + + /* only send in HIL mode */ + if (mavlink_hil_enabled) { + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + } else { + float rudder, throttle; + + /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ + + // XXX very ugly, needs rework + if (isfinite(act_outputs.output[3]) + && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { + /* throttle is fourth output */ + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); + } else { + /* only three outputs, put throttle on position 4 / index 3 */ + rudder = 0; + throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); + } + + mavlink_msg_hil_controls_send(chan, + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 600.0f, + (act_outputs.output[1] - 1500.0f) / 600.0f, + rudder, + throttle, + (act_outputs.output[4] - 1500.0f) / 600.0f, + (act_outputs.output[5] - 1500.0f) / 600.0f, + (act_outputs.output[6] - 1500.0f) / 600.0f, + (act_outputs.output[7] - 1500.0f) / 600.0f, + mavlink_mode, + 0); + } + } + } } void @@ -482,29 +568,6 @@ l_vehicle_attitude_controls(struct listener *l) "ctrl3 ", actuators.control[3]); } - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - actuators.control[0], - actuators.control[1], - actuators.control[2], - actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } } void diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 9821fc7e5f..7b8d83aa8a 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -32,8 +32,10 @@ * ****************************************************************************/ -/* - * @file Implementation of AR.Drone 1.0 / 2.0 control interface +/** + * @file multirotor_pos_control.c + * + * Skeleton for multirotor position controller */ #include diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index 7795f07acf..c33af1311d 100644 --- a/apps/px4/tests/test_adc.c +++ b/apps/px4/tests/test_adc.c @@ -1,7 +1,6 @@ /**************************************************************************** - * px4/sensors/test_gpio.c * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 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 @@ -13,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -32,9 +31,10 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file test_adc.c + * Test for the analog to digital converter. + */ #include #include @@ -54,91 +54,46 @@ #include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: test_gpio - ****************************************************************************/ - int test_adc(int argc, char *argv[]) { - int fd0; + int fd0 = 0; int ret = 0; - //struct adc_msg_s sample[4],sample2[4],sample3[4],sample4[4],sample5[4],sample6[4],sample7[4],sample8[4],sample9[4]; + #pragma pack(push,1) struct adc_msg4_s { - uint8_t am_channel1; /* The 8-bit ADC Channel */ - int32_t am_data1; /* ADC convert result (4 bytes) */ - uint8_t am_channel2; /* The 8-bit ADC Channel */ - int32_t am_data2; /* ADC convert result (4 bytes) */ - uint8_t am_channel3; /* The 8-bit ADC Channel */ - int32_t am_data3; /* ADC convert result (4 bytes) */ - uint8_t am_channel4; /* The 8-bit ADC Channel */ - int32_t am_data4; /* ADC convert result (4 bytes) */ - } __attribute__((__packed__));; + uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ + int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ + uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ + int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ + uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ + int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ + uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ + int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ + }; + #pragma pack(pop) - struct adc_msg4_s sample1[4], sample2[4]; + struct adc_msg4_s sample1; - size_t readsize; - ssize_t nbytes, nbytes2; - int i; + ssize_t nbytes; int j; int errval; - for (j = 0; j < 1; j++) { - char name[11]; - sprintf(name, "/dev/adc%d", j); - fd0 = open(name, O_RDONLY | O_NONBLOCK); + fd0 = open("/dev/adc0", O_RDONLY | O_NONBLOCK); - if (fd0 < 0) { - printf("ADC: %s open fail\n", name); - return ERROR; + if (fd0 <= 0) { + message("/dev/adc0 open fail: %d\n", errno); + return ERROR; - } else { - printf("Opened %s successfully\n", name); - } + } else { + message("opened /dev/adc0 successfully\n"); + } + usleep(10000); - /* first adc read round */ - readsize = 4 * sizeof(struct adc_msg_s); - up_udelay(10000);//microseconds - nbytes = read(fd0, sample1, readsize); - up_udelay(10000);//microseconds - nbytes2 = read(fd0, sample2, readsize); -// nbytes2 = read(fd0, sample3, readsize); -// nbytes2 = read(fd0, sample4, readsize); -// nbytes2 = read(fd0, sample5, readsize); -// nbytes2 = read(fd0, sample6, readsize); -// nbytes2 = read(fd0, sample7, readsize); -// nbytes2 = read(fd0, sample8, readsize); - //nbytes2 = read(fd0, sample9, readsize); + for (j = 0; j < 10; j++) { + + /* sleep 20 milliseconds */ + usleep(20000); + nbytes = read(fd0, &sample1, sizeof(sample1)); /* Handle unexpected return values */ @@ -146,62 +101,44 @@ int test_adc(int argc, char *argv[]) errval = errno; if (errval != EINTR) { - message("read %s failed: %d\n", - name, errval); + message("reading /dev/adc0 failed: %d\n", errval); errval = 3; goto errout_with_dev; } - message("\tInterrupted read...\n"); + message("\tinterrupted read..\n"); } else if (nbytes == 0) { - message("\tNo data read, Ignoring\n"); + message("\tno data read, ignoring.\n"); + ret = ERROR; } /* Print the sample data on successful return */ else { - int nsamples = nbytes / sizeof(struct adc_msg_s); - - if (nsamples * sizeof(struct adc_msg_s) != nbytes) { - message("\tread size=%d is not a multiple of sample size=%d, Ignoring\n", - nbytes, sizeof(struct adc_msg_s)); + if (nbytes != sizeof(sample1)) { + message("\tsample 1 size %d is not matching struct size %d, ignoring\n", + nbytes, sizeof(sample1)); + ret = ERROR; } else { - message("Sample:"); - for (i = 0; i < 1 ; i++) { - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel1, sample1[i].am_data1); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel2, sample1[i].am_data2); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel3, sample1[i].am_data3); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i, sample1[i].am_channel4, sample1[i].am_data4); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel1, sample2[i].am_data1); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel2, sample2[i].am_data2); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel3, sample2[i].am_data3); - message("Sample:"); - message("%d: channel: %d value: %d\n", - i + 1, sample2[i].am_channel4, sample2[i].am_data4); -// message("%d: channel: %d value: %d\n", -// i, sample9[i].am_channel, sample9[i].am_data); - } + message("CYCLE %d:\n", j); + + message("channel: %d value: %d\n", + (int)sample1.am_channel1, sample1.am_data1); + message("channel: %d value: %d", + (int)sample1.am_channel2, sample1.am_data2); + message("channel: %d value: %d", + (int)sample1.am_channel3, sample1.am_data3); + message("channel: %d value: %d", + (int)sample1.am_channel4, sample1.am_data4); } } + fflush(stdout); } - printf("\t ADC test successful.\n"); + message("\t ADC test successful."); errout_with_dev: diff --git a/apps/px4/tests/test_sensors.c b/apps/px4/tests/test_sensors.c index c801869ab6..dc1f39046b 100644 --- a/apps/px4/tests/test_sensors.c +++ b/apps/px4/tests/test_sensors.c @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +36,7 @@ * @file test_sensors.c * Tests the onboard sensors. * - * The sensors app must not be running when performing this test. + * @author Lorenz Meier */ #include @@ -56,7 +56,10 @@ #include "tests.h" +#include #include +#include +#include /**************************************************************************** * Pre-processor Definitions @@ -70,8 +73,10 @@ * Private Function Prototypes ****************************************************************************/ -//static int lis331(int argc, char *argv[]); -static int mpu6000(int argc, char *argv[]); +static int accel(int argc, char *argv[]); +static int gyro(int argc, char *argv[]); +static int mag(int argc, char *argv[]); +static int baro(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -82,7 +87,10 @@ struct { const char *path; int (* test)(int argc, char *argv[]); } sensors[] = { - {"mpu6000", "/dev/accel", mpu6000}, + {"accel", "/dev/accel", accel}, + {"gyro", "/dev/gyro", gyro}, + {"mag", "/dev/mag", mag}, + {"baro", "/dev/baro", baro}, {NULL, NULL, NULL} }; @@ -95,9 +103,9 @@ struct { ****************************************************************************/ static int -mpu6000(int argc, char *argv[]) +accel(int argc, char *argv[]) { - printf("\tMPU-6000: test start\n"); + printf("\tACCEL: test start\n"); fflush(stdout); int fd; @@ -107,7 +115,7 @@ mpu6000(int argc, char *argv[]) fd = open("/dev/accel", O_RDONLY); if (fd < 0) { - printf("\tMPU-6000: open fail, run first.\n"); + printf("\tACCEL: open fail, run or or first.\n"); return ERROR; } @@ -118,11 +126,11 @@ mpu6000(int argc, char *argv[]) ret = read(fd, &buf, sizeof(buf)); if (ret < 3) { - printf("\tMPU-6000: read1 fail (%d)\n", ret); + printf("\tACCEL: read1 fail (%d)\n", ret); return ERROR; } else { - printf("\tMPU-6000 values: acc: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z);//\tgyro: r:%d\tp:%d\ty:%d\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]); + printf("\tACCEL accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } // /* wait at least 10ms, sensor should have data after no more than 2ms */ @@ -141,7 +149,118 @@ mpu6000(int argc, char *argv[]) /* XXX more tests here */ /* Let user know everything is ok */ - printf("\tOK: MPU-6000 passed all tests successfully\n"); + printf("\tOK: ACCEL passed all tests successfully\n"); + + return OK; +} + +static int +gyro(int argc, char *argv[]) +{ + printf("\tGYRO: test start\n"); + fflush(stdout); + + int fd; + struct gyro_report buf; + int ret; + + fd = open("/dev/gyro", O_RDONLY); + + if (fd < 0) { + printf("\tGYRO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tGYRO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: GYRO passed all tests successfully\n"); + + return OK; +} + +static int +mag(int argc, char *argv[]) +{ + printf("\tMAG: test start\n"); + fflush(stdout); + + int fd; + struct mag_report buf; + int ret; + + fd = open("/dev/mag", O_RDONLY); + + if (fd < 0) { + printf("\tMAG: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tMAG: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tMAG values: x:%8.4f\ty:%8.4f\tz:%8.4f\n", (double)buf.x, (double)buf.y, (double)buf.z); + } + + /* Let user know everything is ok */ + printf("\tOK: MAG passed all tests successfully\n"); + + return OK; +} + +static int +baro(int argc, char *argv[]) +{ + printf("\tBARO: test start\n"); + fflush(stdout); + + int fd; + struct baro_report buf; + int ret; + + fd = open("/dev/baro", O_RDONLY); + + if (fd < 0) { + printf("\tBARO: open fail, run or first.\n"); + return ERROR; + } + + /* wait at least 5 ms, sensor should have data after that */ + usleep(5000); + + /* read data - expect samples */ + ret = read(fd, &buf, sizeof(buf)); + + if (ret < 3) { + printf("\tBARO: read fail (%d)\n", ret); + return ERROR; + + } else { + printf("\tBARO pressure: %8.4f mbar\talt: %8.4f m\ttemp: %8.4f deg C\n", (double)buf.pressure, (double)buf.altitude, (double)buf.temperature); + } + + /* Let user know everything is ok */ + printf("\tOK: BARO passed all tests successfully\n"); return OK; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 3e9f35eaf4..675a8602a4 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -34,9 +34,9 @@ /** * @file sensors.cpp - * @author Lorenz Meier - * * Sensor readout process. + * + * @author Lorenz Meier */ #include diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 0041512358..19ce25553a 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -168,6 +168,7 @@ mixer_load_simple(Mixer::ControlCallback control_cb, uintptr_t cb_handle, int fd /* let's assume we're going to read a simple mixer */ mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); + mixinfo->control_count = inputs; /* first, get the output scaler */ ret = mixer_getline(fd, buf, sizeof(buf)); diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index 89b3974783..c3e039d66b 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -1,9 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * Thomas Gubler + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +37,10 @@ /** * @file subsystem_info.h * Definition of the subsystem info topic. + * + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #ifndef TOPIC_SUBSYSTEM_INFO_H_ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 5b2546911a..2e9a989ad7 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -96,6 +96,7 @@ CONFIGURED_APPS += drivers/stm32 CONFIGURED_APPS += drivers/led CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/px4fmu +CONFIGURED_APPS += drivers/hil # Testing stuff CONFIGURED_APPS += px4/sensors_bringup