diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-29 15:53:11 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-29 15:53:11 +0200 |
commit | 7cd89520cc8846e5ca00f251311e6c4a50b1f7e5 (patch) | |
tree | 87b373384315b8244edc88356cdc56324649e71f /apps | |
parent | 7d87f2b06e1b7ee71c132e84cfda263a5207e4d9 (diff) | |
download | px4-firmware-7cd89520cc8846e5ca00f251311e6c4a50b1f7e5.tar.gz px4-firmware-7cd89520cc8846e5ca00f251311e6c4a50b1f7e5.tar.bz2 px4-firmware-7cd89520cc8846e5ca00f251311e6c4a50b1f7e5.zip |
More black magic put into the attitude estimation - works nicely now
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 2 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_bm.c | 6 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 12 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 1201 |
4 files changed, 10 insertions, 1211 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 88d25bfab..80d2c58f8 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -555,7 +555,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) /* add the removed length from x / y to z, since we induce a scaling issue else */ float total_len = sqrtf(accel_offset[0]*accel_offset[0] + accel_offset[1]*accel_offset[1] + accel_offset[2]*accel_offset[2]); - accel_offset[2] = -(accel_offset[2] + total_len); + accel_offset[2] = accel_offset[2] + total_len; if (param_set(param_find("SENSOR_ACC_XOFF"), &(accel_offset[0]))) { mavlink_log_critical(mavlink_fd, "[commander] Setting X accel offset failed!"); diff --git a/apps/px4/attitude_estimator_bm/attitude_bm.c b/apps/px4/attitude_estimator_bm/attitude_bm.c index cb17a356d..29908ddd5 100644 --- a/apps/px4/attitude_estimator_bm/attitude_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_bm.c @@ -147,9 +147,9 @@ void attitude_blackmagic_init(void) // }; static m_elem kal_gain[12 * 9] = { - 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0 , 0, - 0 , 0 , 0.0006f , 0 , 0 , 0 , 0 , 0 , 0, + 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0, + 0 , 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0, 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0, 0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0, diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 97d7d39b7..1174a7524 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -87,14 +87,14 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul gyro_values.z = raw->gyro_rad_s[2]; float_vect3 accel_values; - accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100; - accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100; - accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100; + accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 120; + accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 120; + accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 120; float_vect3 mag_values; - mag_values.x = raw->magnetometer_ga[0]*456.0f; - mag_values.y = raw->magnetometer_ga[1]*456.0f; - mag_values.z = raw->magnetometer_ga[2]*456.0f; + mag_values.x = raw->magnetometer_ga[0]*1500.0f; + mag_values.y = raw->magnetometer_ga[1]*1500.0f; + mag_values.z = raw->magnetometer_ga[2]*1500.0f; // static int i = 0; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c deleted file mode 100644 index f82aa4c52..000000000 --- a/apps/sensors/sensors.c +++ /dev/null @@ -1,1201 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.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 sensors.c - * Sensor readout process. - */ - -#include <nuttx/config.h> - -#include <fcntl.h> -#include <sys/prctl.h> -#include <poll.h> -#include <nuttx/analog/adc.h> -#include <unistd.h> -#include <stdlib.h> -#include <string.h> -#include <stdbool.h> -#include <stdio.h> -#include <errno.h> -#include <float.h> - -#include <arch/board/up_hrt.h> -#include <arch/board/drv_bma180.h> -#include <arch/board/drv_l3gd20.h> - -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> -#include <drivers/drv_baro.h> - -#include <arch/board/up_adc.h> - -#include <systemlib/systemlib.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> - -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> - -#include "sensors.h" - -#define errno *get_errno_ptr() - -#define SENSOR_INTERVAL_MICROSEC 2000 - -#define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ -#define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ -#define MAGN_HEALTH_COUNTER_LIMIT_ERROR 100 /* 1000 ms downtime at 100 Hz update rate */ -#define BARO_HEALTH_COUNTER_LIMIT_ERROR 50 /* 500 ms downtime at 100 Hz update rate */ -#define ADC_HEALTH_COUNTER_LIMIT_ERROR 10 /* 100 ms downtime at 100 Hz update rate */ - -#define GYRO_HEALTH_COUNTER_LIMIT_OK 5 -#define ACC_HEALTH_COUNTER_LIMIT_OK 5 -#define MAGN_HEALTH_COUNTER_LIMIT_OK 5 -#define BARO_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_HEALTH_COUNTER_LIMIT_OK 5 - -#define ADC_BATTERY_VOLATGE_CHANNEL 10 - -#define BAT_VOL_INITIAL 12.f -#define BAT_VOL_LOWPASS_1 0.99f -#define BAT_VOL_LOWPASS_2 0.01f -#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f - -/* PPM Settings */ -#define PPM_MIN 1000 -#define PPM_MAX 2000 -/* Internal resolution is 10000 */ -#define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2) - -#define PPM_MID (PPM_MIN+PPM_MAX)/2 - -static int sensors_timer_loop_counter = 0; - -/* File descriptors for all sensors */ -static int fd_gyro = -1; -static int fd_gyro_l3gd20 = -1; - -static bool thread_should_exit = false; -static bool thread_running = false; -static int sensors_task; - -static int fd_bma180 = -1; -static int fd_magnetometer = -1; -static int fd_barometer = -1; -static int fd_adc = -1; -static int fd_accelerometer = -1; - -/* Private functions declared static */ -static void sensors_timer_loop(void *arg); - -#ifdef CONFIG_HRT_PPM -extern uint16_t ppm_buffer[]; -extern unsigned ppm_decoded_channels; -extern uint64_t ppm_last_valid_decode; -#endif - -/* ORB topic publishing our results */ -static orb_advert_t sensor_pub; - -PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f); - -PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f); - -PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f); -PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f); - -PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); -PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); -PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); -PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC2_MIN, 1000); -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC2_MAX, 2000); -PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC3_MIN, 1000); -PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC3_MAX, 2000); -PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC4_MIN, 1000); -PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC4_MAX, 2000); -PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC5_MIN, 1000); -PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC5_MAX, 2000); -PARAM_DEFINE_FLOAT(RC5_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC6_MIN, 1000); -PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC6_MAX, 2000); -PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC7_MIN, 1000); -PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC7_MAX, 2000); -PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); - -PARAM_DEFINE_FLOAT(RC8_MIN, 1000); -PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC8_MAX, 2000); -PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); - -PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA - -PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); - -PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); -PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); -PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); -PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); - -#define rc_max_chan_count 8 - -struct sensor_parameters { - int min[rc_max_chan_count]; - int trim[rc_max_chan_count]; - int max[rc_max_chan_count]; - int rev[rc_max_chan_count]; - - float gyro_offset[3]; - float mag_offset[3]; - float acc_offset[3]; - - int rc_type; - - int rc_map_roll; - int rc_map_pitch; - int rc_map_yaw; - int rc_map_throttle; - int rc_map_mode_sw; - - int battery_voltage_scaling; -}; - -struct sensor_parameter_handles { - param_t min[rc_max_chan_count]; - param_t trim[rc_max_chan_count]; - param_t max[rc_max_chan_count]; - param_t rev[rc_max_chan_count]; - param_t rc_type; - - param_t gyro_offset[3]; - param_t mag_offset[3]; - param_t acc_offset[3]; - - param_t rc_map_roll; - param_t rc_map_pitch; - param_t rc_map_yaw; - param_t rc_map_throttle; - param_t rc_map_mode_sw; - - param_t battery_voltage_scaling; -}; - -/** - * Sensor app start / stop handling function - * - * @ingroup apps - */ -__EXPORT int sensors_main(int argc, char *argv[]); - -/** - * Sensor readout and publishing. - * - * This function reads all onboard sensors and publishes the sensor_combined topic. - * - * @see sensor_combined_s - */ -int sensors_thread_main(int argc, char *argv[]); - -/** - * Print the usage - */ -static void usage(const char *reason); - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct sensor_parameter_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct sensor_parameter_handles *h, struct sensor_parameters *p); - - -static int parameters_init(struct sensor_parameter_handles *h) -{ - /* min values */ - h->min[0] = param_find("RC1_MIN"); - h->min[1] = param_find("RC2_MIN"); - h->min[2] = param_find("RC3_MIN"); - h->min[3] = param_find("RC4_MIN"); - h->min[4] = param_find("RC5_MIN"); - h->min[5] = param_find("RC6_MIN"); - h->min[6] = param_find("RC7_MIN"); - h->min[7] = param_find("RC8_MIN"); - - /* trim values */ - h->trim[0] = param_find("RC1_TRIM"); - h->trim[1] = param_find("RC2_TRIM"); - h->trim[2] = param_find("RC3_TRIM"); - h->trim[3] = param_find("RC4_TRIM"); - h->trim[4] = param_find("RC5_TRIM"); - h->trim[5] = param_find("RC6_TRIM"); - h->trim[6] = param_find("RC7_TRIM"); - h->trim[7] = param_find("RC8_TRIM"); - - /* max values */ - h->max[0] = param_find("RC1_MAX"); - h->max[1] = param_find("RC2_MAX"); - h->max[2] = param_find("RC3_MAX"); - h->max[3] = param_find("RC4_MAX"); - h->max[4] = param_find("RC5_MAX"); - h->max[5] = param_find("RC6_MAX"); - h->max[6] = param_find("RC7_MAX"); - h->max[7] = param_find("RC8_MAX"); - - /* channel reverse */ - h->rev[0] = param_find("RC1_REV"); - h->rev[1] = param_find("RC2_REV"); - h->rev[2] = param_find("RC3_REV"); - h->rev[3] = param_find("RC4_REV"); - h->rev[4] = param_find("RC5_REV"); - h->rev[5] = param_find("RC6_REV"); - h->rev[6] = param_find("RC7_REV"); - h->rev[7] = param_find("RC8_REV"); - - h->rc_type = param_find("RC_TYPE"); - - h->rc_map_roll = param_find("RC_MAP_ROLL"); - h->rc_map_pitch = param_find("RC_MAP_PITCH"); - h->rc_map_yaw = param_find("RC_MAP_YAW"); - h->rc_map_throttle = param_find("RC_MAP_THROTTLE"); - h->rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); - - /* gyro offsets */ - h->gyro_offset[0] = param_find("SENSOR_GYRO_XOFF"); - h->gyro_offset[1] = param_find("SENSOR_GYRO_YOFF"); - h->gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF"); - - /* accel offsets */ - h->acc_offset[0] = param_find("SENSOR_ACC_XOFF"); - h->acc_offset[1] = param_find("SENSOR_ACC_YOFF"); - h->acc_offset[2] = param_find("SENSOR_ACC_ZOFF"); - - /* mag offsets */ - h->mag_offset[0] = param_find("SENSOR_MAG_XOFF"); - h->mag_offset[1] = param_find("SENSOR_MAG_YOFF"); - h->mag_offset[2] = param_find("SENSOR_MAG_ZOFF"); - - h->battery_voltage_scaling = param_find("BAT_V_SCALING"); - - return OK; -} - -static int parameters_update(const struct sensor_parameter_handles *h, struct sensor_parameters *p) -{ - const unsigned int nchans = 8; - - /* min values */ - for (unsigned int i = 0; i < nchans; i++) { - param_get(h->min[i], &(p->min[i])); - } - - /* trim values */ - for (unsigned int i = 0; i < nchans; i++) { - param_get(h->trim[i], &(p->trim[i])); - } - - /* max values */ - for (unsigned int i = 0; i < nchans; i++) { - param_get(h->max[i], &(p->max[i])); - } - - /* channel reverse */ - for (unsigned int i = 0; i < nchans; i++) { - param_get(h->rev[i], &(p->rev[i])); - } - - /* remote control type */ - param_get(h->rc_type, &(p->rc_type)); - - /* channel mapping */ - param_get(h->rc_map_roll, &(p->rc_map_roll)); - param_get(h->rc_map_pitch, &(p->rc_map_pitch)); - param_get(h->rc_map_yaw, &(p->rc_map_yaw)); - param_get(h->rc_map_throttle, &(p->rc_map_throttle)); - if (param_get(h->rc_map_mode_sw, &(p->rc_map_mode_sw)) != OK) { - warnx("Loading RC mode sw param failed."); - } - - /* gyro offsets */ - param_get(h->gyro_offset[0], &(p->gyro_offset[0])); - param_get(h->gyro_offset[1], &(p->gyro_offset[1])); - param_get(h->gyro_offset[2], &(p->gyro_offset[2])); - - /* accel offsets */ - param_get(h->acc_offset[0], &(p->acc_offset[0])); - param_get(h->acc_offset[1], &(p->acc_offset[1])); - param_get(h->acc_offset[2], &(p->acc_offset[2])); - - /* mag offsets */ - param_get(h->mag_offset[0], &(p->mag_offset[0])); - param_get(h->mag_offset[1], &(p->mag_offset[1])); - param_get(h->mag_offset[2], &(p->mag_offset[2])); - - /* scaling of ADC ticks to battery voltage */ - if (param_get(h->battery_voltage_scaling, &(p->battery_voltage_scaling)) != OK) { - warnx("Loading voltage scaling param failed."); - } - - return OK; -} - -/** - * Initialize all sensor drivers. - * - * @return 0 on success, != 0 on failure - */ -static int sensors_init(void) -{ - printf("[sensors] Sensor configuration..\n"); - - /* open magnetometer */ - fd_magnetometer = open("/dev/mag", O_RDONLY); - - if (fd_magnetometer < 0) { - fprintf(stderr, "[sensors] MAG open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - errno = ENOSYS; - return ERROR; - - } else { - printf("[sensors] MAG open ok\n"); - // /* set the queue depth to 1 */ - // if (OK != ioctl(fd_magnetometer, MAGIOCSQUEUEDEPTH, 1)) - // warn("failed to set queue depth for mag"); - - /* start the sensor polling at 150Hz */ - if (OK != ioctl(fd_magnetometer, MAGIOCSSAMPLERATE, 150)) - warn("failed to set minimum 150Hz sample rate for mag"); - } - - /* open barometer */ - fd_barometer = open("/dev/baro", O_RDONLY); - - if (fd_barometer < 0) { - fprintf(stderr, "[sensors] BARO open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - - } else { - printf("[sensors] BARO open ok\n"); - // /* set the queue depth to 1 */ - // if (OK != ioctl(fd_barometer, BAROIOCSQUEUEDEPTH, 1)) - // warn("failed to set queue depth for baro"); - - // start the sensor polling at 100Hz - // if (OK != ioctl(fd_barometer, BAROIOCSPOLLRATE, 100)) - // warn("failed to set 100Hz poll rate for baro"); - } - - /* open gyro */ - fd_gyro = open("/dev/gyro", O_RDONLY); - int errno_gyro = (int)*get_errno_ptr(); - - if (!(fd_gyro < 0)) { - printf("[sensors] GYRO open ok\n"); - // /* set the queue depth to 1 */ - // if (OK != ioctl(fd_gyro, GYROIOCSQUEUEDEPTH, 1)) - // warn("failed to set queue depth for gyro"); - - /* start the sensor polling at 500Hz */ - if (OK != ioctl(fd_gyro, GYROIOCSSAMPLERATE, 500)) - warn("failed to set minimum 500Hz sample rate for gyro"); - } - - /* open accelerometer */ - fd_accelerometer = open("/dev/accel", O_RDONLY); - int errno_accelerometer = (int)*get_errno_ptr(); - - if (!(fd_accelerometer < 0)) { - printf("[sensors] ACCEL open ok\n"); - // /* set the queue depth to 1 */ - // if (OK != ioctl(fd_accelerometer, ACCELIOCSQUEUEDEPTH, 1)) - // warn("failed to set queue depth for accel"); - - /* start the sensor polling at 500Hz */ - if (OK != ioctl(fd_accelerometer, ACCELIOCSSAMPLERATE, 500)) - warn("failed to set minimum 500Hz poll rate for accel"); - } - - /* only attempt to use BMA180 if main accel is not available */ - int errno_bma180 = 0; - if (fd_accelerometer < 0) { - fd_bma180 = open("/dev/bma180", O_RDONLY); - errno_bma180 = (int)*get_errno_ptr(); - - if (!(fd_bma180 < 0)) { - printf("[sensors] ACCEL (BMA180) open ok\n"); - } - } else { - fd_bma180 = -1; - } - - /* only attempt to use L3GD20 is main gyro is not available */ - int errno_gyro_l3gd20 = 0; - if (fd_gyro < 0) { - fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY); - int errno_gyro_l3gd20 = (int)*get_errno_ptr(); - - if (!(fd_gyro_l3gd20 < 0)) { - printf("[sensors] GYRO (L3GD20) open ok\n"); - } - - if (ioctl(fd_gyro_l3gd20 , L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || - ioctl(fd_gyro_l3gd20 , L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { - fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - errno = ENOSYS; - return ERROR; - - } else { - printf("[sensors] L3GD20 configuration ok\n"); - } - } else { - fd_gyro_l3gd20 = -1; - } - - /* fail if no accelerometer is available */ - if (fd_accelerometer < 0 && fd_bma180 < 0) { - /* print error message only if both failed, discard message else at all to not confuse users */ - if (fd_accelerometer < 0) { - fprintf(stderr, "[sensors] ACCEL: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); - fflush(stderr); - /* this sensor is redundant with BMA180 */ - } - - if (fd_bma180 < 0) { - fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180)); - fflush(stderr); - /* this sensor is redundant with MPU-6000 */ - } - - errno = ENOSYS; - return ERROR; - } - - /* fail if no gyro is available */ - if (fd_gyro < 0 && fd_gyro_l3gd20 < 0) { - /* print error message only if both failed, discard message else at all to not confuse users */ - if (fd_gyro < 0) { - fprintf(stderr, "[sensors] GYRO: open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro)); - fflush(stderr); - /* this sensor is redundant with BMA180 */ - } - - if (fd_gyro_l3gd20 < 0) { - fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", errno_gyro_l3gd20, strerror(errno_gyro_l3gd20)); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - } - - errno = ENOSYS; - return ERROR; - } - - /* open adc */ - fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK); - - if (fd_adc < 0) { - fprintf(stderr, "[sensors] ADC: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - errno = ENOSYS; - return ERROR; - - } else { - printf("[sensors] ADC open ok\n"); - } - - printf("[sensors] All sensors configured\n"); - return OK; -} - -int sensors_thread_main(int argc, char *argv[]) -{ - /* inform about start */ - printf("[sensors] Initializing..\n"); - fflush(stdout); - - int ret = OK; - - /* start sensor reading */ - if (sensors_init() != OK) { - fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors, exiting.\n"); - /* Clean up */ - close(fd_gyro); - close(fd_bma180); - close(fd_gyro_l3gd20); - close(fd_magnetometer); - close(fd_barometer); - close(fd_adc); - - exit(1); - } else { - /* flush stdout from init routine */ - fflush(stdout); - } - - /* initialize parameters */ - struct sensor_parameters rcp; - struct sensor_parameter_handles rch; - parameters_init(&rch); - parameters_update(&rch, &rcp); - - // bool gyro_healthy = false; - // bool acc_healthy = false; - // bool magn_healthy = false; - // bool baro_healthy = false; - // bool adc_healthy = false; - - bool hil_enabled = false; /**< HIL is disabled by default */ - bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */ - - // unsigned int mag_fail_count = 0; - // unsigned int mag_success_count = 0; - - // unsigned int baro_fail_count = 0; - // unsigned int baro_success_count = 0; - - // unsigned int gyro_fail_count = 0; - // unsigned int gyro_success_count = 0; - - // unsigned int acc_fail_count = 0; - // unsigned int acc_success_count = 0; - - // unsigned int adc_fail_count = 0; - // unsigned int adc_success_count = 0; - - /* for PX4FMU 1.5 compatibility */ - int16_t buf_accelerometer[3]; - int16_t buf_gyro[3]; - - // bool mag_calibration_enabled = false; - - #pragma pack(push,1) - struct adc_msg4_s { - 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 buf_adc; - size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); - - float battery_voltage_conversion; - battery_voltage_conversion = rcp.battery_voltage_scaling; - - if (-1 == (int)battery_voltage_conversion) { - /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ - battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; - } - - /* initialize to 100 to execute immediately */ - int paramcounter = 100; - int read_loop_counter = 0; - - /* Empty sensor buffers, avoid junk values */ - /* Read first two values of each sensor into void */ - if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, sizeof(buf_accelerometer)); - if (fd_gyro_l3gd20 > 0)(void)read(fd_gyro_l3gd20, &buf_gyro, sizeof(buf_gyro)); - - /* ORB sensor subscriptions */ - int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - int accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - int mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - int baro_sub = orb_subscribe(ORB_ID(sensor_baro)); - - struct gyro_report gyro_report; - struct accel_report accel_report; - struct mag_report mag_report; - struct baro_report baro_report; - - struct sensor_combined_s raw = { - .timestamp = hrt_absolute_time(), - .gyro_raw = {gyro_report.x_raw, gyro_report.y_raw, gyro_report.z_raw}, - .gyro_raw_counter = 0, - .gyro_rad_s = {gyro_report.x, gyro_report.y, gyro_report.z}, - .accelerometer_raw = {accel_report.x_raw, accel_report.y_raw, accel_report.z_raw}, - .accelerometer_raw_counter = 0, - .accelerometer_m_s2 = {accel_report.x, accel_report.y, accel_report.z}, - .magnetometer_raw = {mag_report.x_raw, mag_report.y_raw, mag_report.z_raw}, - .magnetometer_ga = {mag_report.x, mag_report.y, mag_report.z}, - .magnetometer_raw_counter = 0, - .baro_pres_mbar = baro_report.pressure, - .baro_alt_meter = baro_report.altitude, - .baro_temp_celcius = baro_report.temperature, - .baro_raw_counter = 0, - .battery_voltage_v = BAT_VOL_INITIAL, - .adc_voltage_v = {0.9f , 0.0f , 0.0f}, - .battery_voltage_counter = 0, - .battery_voltage_valid = false, - }; - - /* advertise the sensor_combined topic and make the initial publication */ - sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - if (sensor_pub < 0) { - fprintf(stderr, "[sensors] ERROR: orb_advertise for topic sensor_combined failed.\n"); - } else { - publishing = true; - } - - /* advertise the manual_control topic */ - struct manual_control_setpoint_s manual_control = { .mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE, - .roll = 0.0f, - .pitch = 0.0f, - .yaw = 0.0f, - .throttle = 0.0f }; - - orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); - - if (manual_control_pub < 0) { - fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); - } - - /* advertise the rc topic */ - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - if (rc_pub < 0) { - fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n"); - } - - /* subscribe to system status */ - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); - - thread_running = true; - - while (!thread_should_exit) { - - bool gyro_updated = false; - - struct pollfd fds[4]; - - /* wait for data to be ready */ - fds[0].fd = gyro_sub; - fds[0].events = POLLIN; - - fds[1].fd = accel_sub; - fds[1].events = POLLIN; - - fds[2].fd = mag_sub; - fds[2].events = POLLIN; - - fds[3].fd = baro_sub; - fds[3].events = POLLIN; - - int pret = poll(fds, 4, 500); - - if (pret <= 0) { - /* do silently nothing */ - } else { - - /* store the time closest to all measurements */ - uint64_t current_time = hrt_absolute_time(); - raw.timestamp = current_time; - - /* Update at 5 Hz */ - if (paramcounter == ((unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)/5)) { - - /* Check HIL state */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); - - /* switching from non-HIL to HIL mode */ - //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !hil_enabled) { - hil_enabled = true; - publishing = false; - - int sens_ret = close(sensor_pub); - if (sens_ret == OK) { - printf("[sensors] Closing sensor pub OK\n"); - } else { - printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret); - } - - /* switching from HIL to non-HIL mode */ - - } else if (!publishing && !hil_enabled) { - /* advertise the topic and make the initial publication */ - sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); - hil_enabled = false; - publishing = true; - } - - /* update parameters */ - parameters_update(&rch, &rcp); - - /* Update RC scalings and function mappings */ - rc.chan[0].scaling_factor = (1.0f / ((rcp.max[0] - rcp.min[0]) / 2.0f) * rcp.rev[0]); - rc.chan[0].mid = rcp.trim[0]; - - rc.chan[1].scaling_factor = (1.0f / ((rcp.max[1] - rcp.min[1]) / 2.0f) * rcp.rev[1]); - rc.chan[1].mid = rcp.trim[1]; - - rc.chan[2].scaling_factor = (1.0f / ((rcp.max[2] - rcp.min[2]) / 2.0f) * rcp.rev[2]); - rc.chan[2].mid = rcp.trim[2]; - - rc.chan[3].scaling_factor = (1.0f / ((rcp.max[3] - rcp.min[3]) / 2.0f) * rcp.rev[3]); - rc.chan[3].mid = rcp.trim[3]; - - rc.chan[4].scaling_factor = (1.0f / ((rcp.max[4] - rcp.min[4]) / 2.0f) * rcp.rev[4]); - rc.chan[4].mid = rcp.trim[4]; - - rc.chan[5].scaling_factor = (1.0f / ((rcp.max[5] - rcp.min[5]) / 2.0f) * rcp.rev[5]); - rc.chan[5].mid = rcp.trim[5]; - - rc.chan[6].scaling_factor = (1.0f / ((rcp.max[6] - rcp.min[6]) / 2.0f) * rcp.rev[6]); - rc.chan[6].mid = rcp.trim[6]; - - rc.chan[7].scaling_factor = (1.0f / ((rcp.max[7] - rcp.min[7]) / 2.0f) * rcp.rev[7]); - rc.chan[7].mid = rcp.trim[7]; - - rc.function[0] = rcp.rc_map_throttle - 1; - rc.function[1] = rcp.rc_map_roll - 1; - rc.function[2] = rcp.rc_map_pitch - 1; - rc.function[3] = rcp.rc_map_yaw - 1; - rc.function[4] = rcp.rc_map_mode_sw - 1; - - paramcounter = 0; - } - paramcounter++; - - /* --- GYRO --- */ - if (fds[0].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); - - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; - - raw.gyro_raw[0] = gyro_report.x_raw; - raw.gyro_raw[1] = gyro_report.y_raw; - raw.gyro_raw[2] = gyro_report.z_raw; - - raw.gyro_raw_counter++; - /* gyro is clocking synchronous data output */ - gyro_updated = true; - } - - /* --- ACCEL --- */ - if (fds[1].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_accel), accel_sub, &accel_report); - - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; - - raw.accelerometer_raw[0] = accel_report.x_raw; - raw.accelerometer_raw[1] = accel_report.y_raw; - raw.accelerometer_raw[2] = accel_report.z_raw; - - raw.accelerometer_raw_counter++; - } - - /* --- MAG --- */ - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_mag), mag_sub, &mag_report); - - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; - - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; - - raw.magnetometer_raw_counter++; - } - - /* --- BARO --- */ - if (fds[3].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_baro), baro_sub, &baro_report); - - raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - - raw.baro_raw_counter++; - } - - // /* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */ - // if (fd_bma180 > 0) { - // /* try reading acc */ - // uint64_t start_acc = hrt_absolute_time(); - // ret_accelerometer = read(fd_bma180, buf_accelerometer, 6); - - // /* ACCELEROMETER */ - // if (ret_accelerometer != 6) { - // acc_fail_count++; - - // if ((acc_fail_count % 500) == 0 || (acc_fail_count > 20 && acc_fail_count < 40)) { - // fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - // } - - // if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { - // // global_data_send_subsystem_info(&acc_present_enabled); - // gyro_healthy = false; - // acc_success_count = 0; - // } - - // } else { - // acc_success_count++; - - // if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { - - // // global_data_send_subsystem_info(&acc_present_enabled_healthy); - // acc_healthy = true; - // acc_fail_count = 0; - - // } - - // acc_updated = true; - // } - - // int acctime = hrt_absolute_time() - start_acc; - // if (acctime > 500) printf("ACC: %d us\n", acctime); - // } - - - // /* ACCELEROMETER */ - // if (acc_updated) { - // /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ - - // if (fd_bma180 > 0) { - - // /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - // raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor - // raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor - // raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor - - - // // XXX read range from sensor - // float range_g = 4.0f; - // /* scale from 14 bit to m/s2 */ - // raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - rcp.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; - // raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - rcp.acc_offset[1]) * range_g) / 8192.0f) / 9.81f; - // raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - rcp.acc_offset[2]) * range_g) / 8192.0f) / 9.81f; - - // raw.accelerometer_raw_counter++; - // } - // } - - // if (fd_gyro_l3gd20 > 0) { - // /* try reading gyro */ - // uint64_t start_gyro = hrt_absolute_time(); - // ret_gyro = read(fd_gyro, buf_gyro_l3gd20, sizeof(buf_gyro_l3gd20)); - // int gyrotime = hrt_absolute_time() - start_gyro; - - // if (gyrotime > 500) printf("L3GD20 GYRO (pure read): %d us\n", gyrotime); - - // /* GYROSCOPE */ - // if (ret_gyro != sizeof(buf_gyro)) { - // gyro_fail_count++; - - // if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { - // fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - // } - - // if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { - // // global_data_send_subsystem_info(&gyro_present_enabled); - // gyro_healthy = false; - // gyro_success_count = 0; - // } - - // } else { - // gyro_success_count++; - - // if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { - // // global_data_send_subsystem_info(&gyro_present_enabled_healthy); - // gyro_healthy = true; - // gyro_fail_count = 0; - - // } - - // gyro_updated = true; - // } - - // gyrotime = hrt_absolute_time() - start_gyro; - - // if (gyrotime > 500) printf("L3GD20 GYRO (complete): %d us\n", gyrotime); - // } - - /* GYROSCOPE */ - // if (gyro_updated) { - // /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ - - // raw.gyro_raw[0] = ((buf_gyro[1] == -32768) ? -32768 : buf_gyro[1]); // x of the board is y of the sensor - // /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - // raw.gyro_raw[1] = ((buf_gyro[0] == -32768) ? 32767 : -buf_gyro[0]); // y on the board is -x of the sensor - // raw.gyro_raw[2] = ((buf_gyro[2] == -32768) ? -32768 : buf_gyro[2]); // z of the board is z of the sensor - - // /* scale measurements */ - // // XXX request scaling from driver instead of hardcoding it - // /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ - // raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f; - // raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f; - // raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f; - - // raw.gyro_raw_counter++; - // } - - static uint64_t last_adc = 0; - /* ADC */ - if (hrt_absolute_time() - last_adc >= 10000) { - int ret_adc = read(fd_adc, &buf_adc, adc_readsize); - int nsamples_adc = ret_adc / sizeof(struct adc_msg_s); - - // if (ret_adc < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) { - // adc_fail_count++; - - // if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { - // fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - // } - - // if (adc_healthy && adc_fail_count >= ADC_HEALTH_COUNTER_LIMIT_ERROR) { - // adc_healthy = false; - // adc_success_count = 0; - // } - - // } else { - // adc_success_count++; - - // if (!adc_healthy && adc_success_count >= ADC_HEALTH_COUNTER_LIMIT_OK) { - // adc_healthy = true; - // adc_fail_count = 0; - // } - - // adc_updated = true; - // } - - if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) { - /* Voltage in volts */ - raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * battery_voltage_conversion))); - - if ((buf_adc.am_data1 * battery_voltage_conversion) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - raw.battery_voltage_valid = false; - raw.battery_voltage_v = 0.f; - - } else { - raw.battery_voltage_valid = true; - } - - raw.battery_voltage_counter++; - } - - last_adc = hrt_absolute_time(); - } - - /* Inform other processes that new data is available to copy */ - if (gyro_updated && publishing) { - /* Values changed, publish */ - orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw); - } - -#ifdef CONFIG_HRT_PPM - static uint64_t last_ppm = 0; - - /* PPM */ - if (hrt_absolute_time() - last_ppm >= 10000) { - - /* require at least two channels - * to consider the signal valid - * check that decoded measurement is up to date - */ - if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { - /* Read out values from HRT */ - for (unsigned int i = 0; i < ppm_decoded_channels; i++) { - rc.chan[i].raw = ppm_buffer[i]; - /* Set the range to +-, then scale up */ - rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000; - rc.chan[i].scaled = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; - } - - rc.chan_count = ppm_decoded_channels; - rc.timestamp = ppm_last_valid_decode; - - /* roll input */ - manual_control.roll = rc.chan[rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - - /* pitch input */ - manual_control.pitch = rc.chan[rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - - /* yaw input */ - manual_control.yaw = rc.chan[rc.function[YAW]].scaled; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - - /* throttle input */ - manual_control.throttle = (rc.chan[rc.function[THROTTLE]].scaled+1.0f)/2.0f; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - - /* mode switch input */ - manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; - - orb_publish(ORB_ID(rc_channels), rc_pub, &rc); - orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control); - - } - last_ppm = hrt_absolute_time(); - } -#endif - - read_loop_counter++; - } - } - - printf("[sensors] sensor readout stopped\n"); - - close(fd_gyro); - close(fd_magnetometer); - close(fd_barometer); - close(fd_adc); - - /* maintained for backwards-compatibility with v1.5 */ - close(fd_gyro_l3gd20); - close(fd_bma180); - - close(gyro_sub); - close(accel_sub); - close(mag_sub); - close(baro_sub); - - printf("[sensors] exiting.\n"); - - thread_running = false; - - return ret; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: sensors {start|stop|status}\n"); - exit(1); -} - -int sensors_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("sensors app already running\n"); - } else { - thread_should_exit = false; - sensors_task = task_create("sensors", SCHED_PRIORITY_MAX - 5, 4096, sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - } - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - if (!thread_running) { - printf("sensors app not started\n"); - } else { - printf("stopping sensors app\n"); - thread_should_exit = true; - } - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tsensors app is running\n"); - } else { - printf("\tsensors app not started\n"); - } - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - |