diff options
author | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-27 23:01:31 +0530 |
---|---|---|
committer | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-27 23:01:31 +0530 |
commit | d40168dc4b46f922b1be76dcf8a6a37a675788ae (patch) | |
tree | c5b3226d00bfc09bd8d1de114b7231f3b942167b /src | |
parent | c29972424f6d7b99633c8497f0c25ab7cda4d2ca (diff) | |
download | px4-firmware-d40168dc4b46f922b1be76dcf8a6a37a675788ae.tar.gz px4-firmware-d40168dc4b46f922b1be76dcf8a6a37a675788ae.tar.bz2 px4-firmware-d40168dc4b46f922b1be76dcf8a6a37a675788ae.zip |
Add support for rotations of PX4flow
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/drv_px4flow.h | 2 | ||||
-rw-r--r-- | src/drivers/drv_sensor.h | 15 | ||||
-rw-r--r-- | src/drivers/px4flow/px4flow.cpp | 22 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 36 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 32 |
5 files changed, 92 insertions, 15 deletions
diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index ab640837b..5aed3f02b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file Rangefinder driver interface. + * @file PX4Flow driver interface. */ #ifndef _DRV_PX4FLOW_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8e8b2c1b9..5e4598de8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,8 +82,19 @@ #define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) /** - * Reset the sensor to its default configuration. + * Reset the sensor to its default configuration */ #define SENSORIOCRESET _SENSORIOC(4) -#endif /* _DRV_SENSOR_H */
\ No newline at end of file +/** + * Set the sensor orientation + */ +#define SENSORIOCSROTATION _SENSORIOC(5) + +/** + * Get the sensor orientation + */ +#define SENSORIOCGROTATION _SENSORIOC(6) + +#endif /* _DRV_SENSOR_H */ + diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc65..99a9cc02a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -62,6 +62,8 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> +#include <conversion/rotation.h> + #include <drivers/drv_hrt.h> #include <drivers/drv_px4flow.h> #include <drivers/device/ringbuffer.h> @@ -125,7 +127,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0); virtual ~PX4FLOW(); virtual int init(); @@ -154,6 +156,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + + enum Rotation _sensor_rotation; /** * Test whether the device supported by the driver is present at a @@ -199,7 +203,7 @@ private: */ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); -PX4FLOW::PX4FLOW(int bus, int address) : +PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz _reports(nullptr), _sensor_ok(false), @@ -208,7 +212,8 @@ PX4FLOW::PX4FLOW(int bus, int address) : _px4flow_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), + _sensor_rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -358,8 +363,12 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); + case SENSORIOCSROTATION: + _sensor_rotation = (enum Rotation)arg; + return OK; + + case SENSORIOCGROTATION: + return _sensor_rotation; case SENSORIOCRESET: /* XXX implement this */ @@ -535,6 +544,9 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; + + /* rotate measurements to flight controller frame */ + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce..f5f6094d1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -262,6 +262,42 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * PX4Flow board rotation + * + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); + +/** * Board rotation Y (Pitch) offset * * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..d8df2c8ff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -63,6 +63,7 @@ #include <drivers/drv_rc_input.h> #include <drivers/drv_adc.h> #include <drivers/drv_airspeed.h> +#include <drivers/drv_px4flow.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -263,6 +264,7 @@ private: float diff_pres_analog_scale; int board_rotation; + int flow_rotation; int external_mag_rotation; float board_offset[3]; @@ -361,6 +363,7 @@ private: param_t battery_current_scaling; param_t board_rotation; + param_t flow_rotation; param_t external_mag_rotation; param_t board_offset[3]; @@ -614,6 +617,7 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ @@ -831,8 +835,22 @@ Sensors::parameters_update() } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + /* set px4flow rotation */ + int flowfd; + flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + if (flowret) { + warnx("flow rotation could not be set"); + close(flowfd); + return ERROR; + } + close(flowfd); + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -850,20 +868,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int fd; - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { + int barofd; + barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warn("%s", BARO_DEVICE_PATH); errx(1, "FATAL: no barometer found"); } else { - int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - if (ret) { + int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); - close(fd); + close(barofd); return ERROR; } - close(fd); + close(barofd); } return OK; |