px4-firmware/apps/sensors/sensors.cpp

1114 lines
30 KiB
C++
Raw Normal View History

/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @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 sensors.cpp
*
* Sensor readout process.
*/
#include <nuttx/config.h>
#include <fcntl.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 <arch/board/up_hrt.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 <systemlib/perf_counter.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 <uORB/topics/parameter_update.h>
#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
#ifdef CONFIG_HRT_PPM
extern "C" {
extern uint16_t ppm_buffer[];
extern unsigned ppm_decoded_channels;
extern uint64_t ppm_last_valid_decode;
}
/* 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
#endif
/**
* Sensor app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int sensors_main(int argc, char *argv[]);
class Sensors
{
public:
2012-08-25 23:56:29 -03:00
/**
* Constructor
*/
Sensors();
2012-08-25 23:56:29 -03:00
/**
* Destructor, also kills the sensors task.
*/
~Sensors();
2012-08-25 23:56:29 -03:00
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
private:
2012-08-25 23:56:29 -03:00
static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */
/* legacy sensor descriptors */
2012-08-25 23:56:29 -03:00
int _fd_bma180; /**< old accel driver */
int _fd_gyro_l3gd20; /**< old gyro driver */
#if CONFIG_HRT_PPM
2012-08-25 23:56:29 -03:00
hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */
/**
* Gather and publish PPM input data.
*/
void ppm_poll();
#endif
2012-08-25 23:09:10 -03:00
/* XXX should not be here - should be own driver */
2012-08-25 23:56:29 -03:00
int _fd_adc; /**< ADC driver handle */
hrt_abstime _last_adc; /**< last time we took input from the ADC */
2012-08-25 23:56:29 -03:00
bool _task_should_exit; /**< if true, sensor task should exit */
int _sensors_task; /**< task handle for sensor task */
2012-08-25 23:56:29 -03:00
bool _hil_enabled; /**< if true, HIL is active */
bool _publishing; /**< if true, we are publishing sensor data */
2012-08-25 23:56:29 -03:00
int _gyro_sub; /**< raw gyro data subscription */
int _accel_sub; /**< raw accel data subscription */
int _mag_sub; /**< raw mag data subscription */
int _baro_sub; /**< raw baro data subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
2012-08-25 23:56:29 -03:00
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
2012-08-25 23:56:29 -03:00
perf_counter_t _loop_perf; /**< loop performance counter */
2012-08-25 23:56:29 -03:00
struct rc_channels_s _rc; /**< r/c channel data */
struct {
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;
float battery_voltage_scaling;
2012-08-25 23:56:29 -03:00
} _parameters; /**< local copies of interesting parameters */
struct {
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;
2012-08-25 23:56:29 -03:00
} _parameter_handles; /**< handles for interesting parameters */
2012-08-25 23:56:29 -03:00
/**
* Update our local parameter cache.
*/
int parameters_update();
2012-08-25 23:56:29 -03:00
/**
* Do accel-related initialisation.
*/
void accel_init();
2012-08-25 23:56:29 -03:00
/**
* Do gyro-related initialisation.
*/
void gyro_init();
2012-08-25 23:56:29 -03:00
/**
* Do mag-related initialisation.
*/
void mag_init();
2012-08-25 23:56:29 -03:00
/**
* Do baro-related initialisation.
*/
void baro_init();
2012-08-25 23:56:29 -03:00
/**
* Do adc-related initialisation.
*/
void adc_init();
2012-08-25 23:56:29 -03:00
/**
* Poll the accelerometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void accel_poll(struct sensor_combined_s &raw);
2012-08-25 23:56:29 -03:00
/**
* Poll the gyro for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void gyro_poll(struct sensor_combined_s &raw);
2012-08-25 23:56:29 -03:00
/**
* Poll the magnetometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void mag_poll(struct sensor_combined_s &raw);
2012-08-25 23:56:29 -03:00
/**
* Poll the barometer for updated data.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void baro_poll(struct sensor_combined_s &raw);
2012-08-25 23:56:29 -03:00
/**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
2012-08-25 23:56:29 -03:00
/**
* Check for changes in parameters.
*/
void parameter_update_poll();
2012-08-25 23:56:29 -03:00
/**
* Poll the ADC and update readings to suit.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
2012-08-25 23:09:10 -03:00
void adc_poll(struct sensor_combined_s &raw);
2012-08-25 23:56:29 -03:00
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
2012-08-25 23:56:29 -03:00
/**
* Main sensor collection task.
*/
void task_main() __attribute__((noreturn));
};
namespace sensors
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Sensors *g_sensors;
}
Sensors::Sensors() :
_fd_bma180(-1),
_fd_gyro_l3gd20(-1),
_ppm_last_valid(0),
2012-08-25 23:09:10 -03:00
_fd_adc(-1),
_last_adc(0),
_task_should_exit(false),
_sensors_task(-1),
_hil_enabled(false),
_publishing(true),
/* subscriptions */
_gyro_sub(-1),
_accel_sub(-1),
_mag_sub(-1),
_baro_sub(-1),
_vstatus_sub(-1),
_params_sub(-1),
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_rc_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
{
2012-08-25 23:56:29 -03:00
/* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
char nbuf[16];
2012-08-25 23:56:29 -03:00
/* min values */
sprintf(nbuf, "RC%d_MIN", i + 1);
_parameter_handles.min[i] = param_find(nbuf);
2012-08-25 23:56:29 -03:00
/* trim values */
sprintf(nbuf, "RC%d_TRIM", i + 1);
_parameter_handles.trim[i] = param_find(nbuf);
2012-08-25 23:56:29 -03:00
/* max values */
sprintf(nbuf, "RC%d_MAX", i + 1);
_parameter_handles.max[i] = param_find(nbuf);
/* channel reverse */
sprintf(nbuf, "RC%d_REV", i + 1);
_parameter_handles.rev[i] = param_find(nbuf);
}
_parameter_handles.rc_type = param_find("RC_TYPE");
_parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL");
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENSOR_GYRO_XOFF");
_parameter_handles.gyro_offset[1] = param_find("SENSOR_GYRO_YOFF");
_parameter_handles.gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF");
/* accel offsets */
_parameter_handles.acc_offset[0] = param_find("SENSOR_ACC_XOFF");
_parameter_handles.acc_offset[1] = param_find("SENSOR_ACC_YOFF");
_parameter_handles.acc_offset[2] = param_find("SENSOR_ACC_ZOFF");
/* mag offsets */
_parameter_handles.mag_offset[0] = param_find("SENSOR_MAG_XOFF");
_parameter_handles.mag_offset[1] = param_find("SENSOR_MAG_YOFF");
_parameter_handles.mag_offset[2] = param_find("SENSOR_MAG_ZOFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* fetch initial parameter values */
parameters_update();
}
Sensors::~Sensors()
{
if (_sensors_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
2012-08-25 23:56:29 -03:00
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_sensors_task);
break;
}
} while (_sensors_task != -1);
}
sensors::g_sensors = nullptr;
}
int
Sensors::parameters_update()
{
const unsigned int nchans = 8;
/* min values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.min[i], &(_parameters.min[i]));
}
/* trim values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.trim[i], &(_parameters.trim[i]));
}
/* max values */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.max[i], &(_parameters.max[i]));
}
/* channel reverse */
for (unsigned int i = 0; i < nchans; i++) {
param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
}
/* remote control type */
param_get(_parameter_handles.rc_type, &(_parameters.rc_type));
/* channel mapping */
param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll));
param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch));
param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw));
param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle));
param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw));
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
/* accel offsets */
param_get(_parameter_handles.acc_offset[0], &(_parameters.acc_offset[0]));
param_get(_parameter_handles.acc_offset[1], &(_parameters.acc_offset[1]));
param_get(_parameter_handles.acc_offset[2], &(_parameters.acc_offset[2]));
/* mag offsets */
param_get(_parameter_handles.mag_offset[0], &(_parameters.mag_offset[0]));
param_get(_parameter_handles.mag_offset[1], &(_parameters.mag_offset[1]));
param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("Failed updating voltage scaling param");
}
return OK;
}
void
Sensors::accel_init()
{
int fd;
fd = open(ACCEL_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", ACCEL_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_bma180 = open("/dev/bma180", O_RDONLY);
if (_fd_bma180 < 0) {
warn("/dev/bma180");
errx(1, "FATAL: no accelerometer found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_bma180, junk_buf, sizeof(junk_buf));
warnx("using BMA180");
} else {
/* set the accel internal sampling rate up to at leat 500Hz */
if (OK != ioctl(fd, ACCELIOCSSAMPLERATE, 500))
warn("WARNING: failed to set minimum 500Hz sample rate for accel");
/* set the driver to poll at 500Hz */
2012-08-25 22:31:12 -03:00
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500))
warn("WARNING: failed to set 500Hz poll rate for accel");
warnx("using system accel");
close(fd);
}
}
void
Sensors::gyro_init()
{
int fd;
fd = open(GYRO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", GYRO_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
if (_fd_gyro_l3gd20 < 0) {
warn("/dev/l3gd20");
errx(1, "FATAL: no gyro found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_gyro_l3gd20, junk_buf, sizeof(junk_buf));
warn("using L3GD20");
} else {
/* set the gyro internal sampling rate up to at leat 500Hz */
if (OK != ioctl(fd, GYROIOCSSAMPLERATE, 500))
warn("WARNING: failed to set minimum 500Hz sample rate for gyro");
/* set the driver to poll at 500Hz */
2012-08-25 22:31:12 -03:00
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 500))
warn("WARNING: failed to set 500Hz poll rate for gyro");
warnx("using system gyro");
close(fd);
}
}
void
Sensors::mag_init()
{
int fd;
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", MAG_DEVICE_PATH);
errx(1, "FATAL: no magnetometer found");
}
/* set the mag internal poll rate to at least 150Hz */
if (OK != ioctl(fd, MAGIOCSSAMPLERATE, 150))
warn("WARNING: failed to set minimum 150Hz sample rate for mag");
/* set the driver to poll at 150Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150))
warn("WARNING: failed to set 150Hz poll rate for mag");
close(fd);
}
void
Sensors::baro_init()
{
int fd;
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
errx(1, "FATAL: no barometer found");
}
/* set the driver to poll at 150Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 150))
warn("WARNING: failed to set 150Hz poll rate for baro");
close(fd);
}
void
Sensors::adc_init()
{
_fd_adc = open("/dev/adc0", O_RDONLY | O_NONBLOCK);
if (_fd_adc < 0) {
warn("/dev/adc0");
errx(1, "FATAL: no ADC found");
}
}
void
Sensors::accel_poll(struct sensor_combined_s &raw)
{
struct accel_report accel_report;
if (_fd_bma180 >= 0) {
/* do ORB emulation for BMA180 */
int16_t buf[3];
read(_fd_bma180, buf, sizeof(buf));
accel_report.timestamp = hrt_absolute_time();
accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
accel_report.y_raw = buf[0];
accel_report.z_raw = buf[2];
const float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
} else {
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++;
}
void
Sensors::gyro_poll(struct sensor_combined_s &raw)
{
struct gyro_report gyro_report;
if (_fd_gyro_l3gd20 >= 0) {
/* do ORB emulation for L3GD20 */
int16_t buf[3];
read(_fd_gyro_l3gd20, buf, sizeof(buf));
gyro_report.timestamp = hrt_absolute_time();
gyro_report.x_raw = buf[1];
gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]);
gyro_report.z_raw = buf[2];
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f;
gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
} else {
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++;
}
}
void
Sensors::mag_poll(struct sensor_combined_s &raw)
{
struct mag_report mag_report;
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++;
}
void
Sensors::baro_poll(struct sensor_combined_s &raw)
{
struct baro_report baro_report;
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++;
}
void
Sensors::vehicle_status_poll()
{
struct vehicle_status_s vstatus;
bool vstatus_updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
if (vstatus_updated) {
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;
/* switching from HIL to non-HIL mode */
} else if (!_publishing && !_hil_enabled) {
_hil_enabled = false;
_publishing = true;
}
}
}
void
Sensors::parameter_update_poll()
{
bool param_updated;
/* Check if any parameter has changed */
orb_check(_params_sub, &param_updated);
if (param_updated)
{
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
printf("PARAMS UPDATED\n");
/* update parameters */
parameters_update();
2012-08-27 18:43:56 -03:00
/* update sensor offsets */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
_parameters.gyro_offset[0],
1.0f,
_parameters.gyro_offset[1],
1.0f,
_parameters.gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
_parameters.acc_offset[0],
1.0f,
_parameters.acc_offset[1],
1.0f,
_parameters.acc_offset[2],
1.0f,
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale = {
_parameters.mag_offset[0],
1.0f,
_parameters.mag_offset[1],
1.0f,
_parameters.mag_offset[2],
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
/* update RC scalings and function mappings */
_rc.chan[0].scaling_factor = (1.0f / ((_parameters.max[0] - _parameters.min[0]) / 2.0f) * _parameters.rev[0]);
_rc.chan[0].mid = _parameters.trim[0];
_rc.chan[1].scaling_factor = (1.0f / ((_parameters.max[1] - _parameters.min[1]) / 2.0f) * _parameters.rev[1]);
_rc.chan[1].mid = _parameters.trim[1];
_rc.chan[2].scaling_factor = (1.0f / ((_parameters.max[2] - _parameters.min[2]) / 2.0f) * _parameters.rev[2]);
_rc.chan[2].mid = _parameters.trim[2];
_rc.chan[3].scaling_factor = (1.0f / ((_parameters.max[3] - _parameters.min[3]) / 2.0f) * _parameters.rev[3]);
_rc.chan[3].mid = _parameters.trim[3];
_rc.chan[4].scaling_factor = (1.0f / ((_parameters.max[4] - _parameters.min[4]) / 2.0f) * _parameters.rev[4]);
_rc.chan[4].mid = _parameters.trim[4];
_rc.chan[5].scaling_factor = (1.0f / ((_parameters.max[5] - _parameters.min[5]) / 2.0f) * _parameters.rev[5]);
_rc.chan[5].mid = _parameters.trim[5];
_rc.chan[6].scaling_factor = (1.0f / ((_parameters.max[6] - _parameters.min[6]) / 2.0f) * _parameters.rev[6]);
_rc.chan[6].mid = _parameters.trim[6];
_rc.chan[7].scaling_factor = (1.0f / ((_parameters.max[7] - _parameters.min[7]) / 2.0f) * _parameters.rev[7]);
_rc.chan[7].mid = _parameters.trim[7];
_rc.function[0] = _parameters.rc_map_throttle - 1;
_rc.function[1] = _parameters.rc_map_roll - 1;
_rc.function[2] = _parameters.rc_map_pitch - 1;
_rc.function[3] = _parameters.rc_map_yaw - 1;
_rc.function[4] = _parameters.rc_map_mode_sw - 1;
}
}
2012-08-25 23:09:10 -03:00
void
Sensors::adc_poll(struct sensor_combined_s &raw)
{
#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) */
} buf_adc;
#pragma pack(pop)
if (hrt_absolute_time() - _last_adc >= 10000) {
read(_fd_adc, &buf_adc, sizeof(buf_adc));
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 * _parameters.battery_voltage_scaling)));
if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < 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();
}
}
#if CONFIG_HRT_PPM
void
Sensors::ppm_poll()
{
struct manual_control_setpoint_s manual_control;
/* check to see whether a new frame has been decoded */
if (_ppm_last_valid == ppm_last_valid_decode)
return;
/* require at least two chanels to consider the signal valid */
if (ppm_decoded_channels < 2)
return;
/* we are accepting this decode */
_ppm_last_valid = ppm_last_valid_decode;
/* 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);
}
#endif
void
Sensors::task_main_trampoline(int argc, char *argv[])
{
sensors::g_sensors->task_main();
}
void
Sensors::task_main()
{
/* inform about start */
printf("[sensors] Initializing..\n");
fflush(stdout);
/* start individual sensors */
accel_init();
gyro_init();
mag_init();
baro_init();
adc_init();
/*
* do subscriptions
*/
_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200);
/*
* do advertisements
*/
struct sensor_combined_s raw;
raw.timestamp = hrt_absolute_time();
raw.battery_voltage_v = BAT_VOL_INITIAL;
raw.adc_voltage_v[0] = 0.9f;
raw.adc_voltage_v[1] = 0.0f;
raw.adc_voltage_v[2] = 0.0f;
raw.battery_voltage_counter = 0;
raw.battery_voltage_valid = false;
/* get a set of initial values */
accel_poll(raw);
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
/* advertise the sensor_combined topic and make the initial publication */
_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
/* advertise the manual_control topic */
{
struct manual_control_setpoint_s manual_control;
manual_control.mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE;
manual_control.roll = 0.0f;
manual_control.pitch = 0.0f;
manual_control.yaw = 0.0f;
manual_control.throttle = 0.0f;
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
/* advertise the rc topic */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
}
2012-08-25 23:09:10 -03:00
/* wakeup source(s) */
struct pollfd fds[1];
/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
fds[0].fd = _gyro_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
if (pret == 0)
continue;
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
vehicle_status_poll();
/* check parameters for updates */
parameter_update_poll();
/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
raw.timestamp = hrt_absolute_time();
/* copy most recent sensor data */
accel_poll(raw);
gyro_poll(raw);
mag_poll(raw);
baro_poll(raw);
/* check battery voltage */
2012-08-25 23:09:10 -03:00
adc_poll(raw);
/* Inform other processes that new data is available to copy */
if (_publishing)
orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
#ifdef CONFIG_HRT_PPM
2012-08-25 23:09:10 -03:00
/* Look for new r/c input data */
ppm_poll();
#endif
perf_end(_loop_perf);
}
printf("[sensors] exiting.\n");
_sensors_task = -1;
_exit(0);
}
int
Sensors::start()
{
ASSERT(_sensors_task == -1);
/* start the task */
_sensors_task = task_create("sensor_task",
SCHED_PRIORITY_MAX - 5,
2012-08-25 23:56:29 -03:00
4096, /* XXX may be excesssive */
(main_t)&Sensors::task_main_trampoline,
nullptr);
if (_sensors_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
int sensors_main(int argc, char *argv[])
{
if (argc < 1)
errx(1, "usage: sensors {start|stop|status}");
if (!strcmp(argv[1], "start")) {
if (sensors::g_sensors != nullptr)
errx(1, "sensors task already running");
sensors::g_sensors = new Sensors;
if (sensors::g_sensors == nullptr)
errx(1, "sensors task alloc failed");
2012-08-25 23:56:29 -03:00
if (OK != sensors::g_sensors->start()) {
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
err(1, "sensors task start failed");
2012-08-25 23:56:29 -03:00
}
exit(0);
}
if (!strcmp(argv[1], "stop")) {
if (sensors::g_sensors == nullptr)
errx(1, "sensors task not running");
delete sensors::g_sensors;
sensors::g_sensors = nullptr;
exit(0);
}
if (!strcmp(argv[1], "status")) {
if (sensors::g_sensors) {
errx(0, "task is running");
} else {
errx(1, "task is not running");
}
}
errx(1, "unrecognized command");
}