From bcdedd9a35a5b9ebf3851a0d472adab8d3e7edac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 18:55:32 +0200 Subject: Changed location of lots of flags and conditions, needs testing and more work --- src/modules/sensors/sensors.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acc0c2717..5e334638f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -165,7 +165,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -352,9 +352,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -411,7 +411,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* 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) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1348,12 +1348,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* * do advertisements @@ -1406,7 +1406,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); -- cgit v1.2.3 From 55fd19f2813110d14d536943503851255c997b6f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 22:37:10 +0400 Subject: sensors: ASSISTED switch channel added --- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++++++++++++- src/modules/uORB/topics/rc_channels.h | 24 ++++++++++++++---------- 3 files changed, 28 insertions(+), 11 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3ed9efc8b..d0af9e17b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,6 +167,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 89d5cd374..b38dc8d89 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -208,6 +208,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; + int rc_map_assisted_sw; int rc_map_mission_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -256,6 +257,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; + param_t rc_map_assisted_sw; param_t rc_map_mission_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -464,6 +466,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -617,6 +620,10 @@ Sensors::parameters_update() warnx("Failed getting return sw chan index"); } + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); + } + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { warnx("Failed getting mission sw chan index"); } @@ -673,6 +680,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1142,6 +1150,7 @@ Sensors::ppm_poll() manual_control.mode_switch = NAN; manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; @@ -1249,7 +1258,10 @@ Sensors::ppm_poll() /* land switch input */ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - /* land switch input */ + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index a0bb25af4..2167e44a2 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -53,9 +53,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -70,14 +73,15 @@ enum RC_CHANNELS_FUNCTION YAW = 3, MODE = 4, RETURN = 5, - MISSION = 6, - OFFBOARD_MODE = 7, - FLAPS = 8, - AUX_1 = 9, - AUX_2 = 10, - AUX_3 = 11, - AUX_4 = 12, - AUX_5 = 13, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; -- cgit v1.2.3 From b9446af3f9758604211aefe64f91e27a7e71c631 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Aug 2013 16:29:43 +0200 Subject: Show correct battery voltage for v2 boards --- src/modules/sensors/sensor_params.c | 6 +++++- src/modules/sensors/sensors.cpp | 31 +++++++++++++++++++++++++++++-- 2 files changed, 34 insertions(+), 3 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 133cda8d6..ba4d2186c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -157,8 +157,12 @@ PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ff9e19b4..d9185891b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,8 +92,35 @@ #define BARO_HEALTH_COUNTER_LIMIT_OK 5 #define ADC_HEALTH_COUNTER_LIMIT_OK 5 -#define ADC_BATTERY_VOLTAGE_CHANNEL 10 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +/** + * Analog layout: + * FMU: + * IN2 - battery voltage + * IN3 - battery current + * IN4 - 5V sense + * IN10 - spare (we could actually trim these from the set) + * IN11 - spare (we could actually trim these from the set) + * IN12 - spare (we could actually trim these from the set) + * IN13 - aux1 + * IN14 - aux2 + * IN15 - pressure sensor + * + * IO: + * IN4 - servo supply rail + * IN5 - analog RSSI + */ + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + #define ADC_BATTERY_VOLTAGE_CHANNEL 10 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + #define ADC_BATTERY_VOLTAGE_CHANNEL 2 + #define ADC_BATTERY_CURRENT_CHANNEL 3 + #define ADC_5V_RAIL_SENSE 4 + #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#endif #define BAT_VOL_INITIAL 0.f #define BAT_VOL_LOWPASS_1 0.99f -- cgit v1.2.3 From c2ee4437e0fd362ed8d73203394d34802e9eb48d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 19:20:55 +0200 Subject: Rate limit sensors, as the in-sensor lowpass allows us to operate at 200-250 Hz --- src/modules/sensors/sensors.cpp | 35 ++++++++++++++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9185891b..0f1782fca 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -758,12 +758,27 @@ Sensors::accel_init() errx(1, "FATAL: no accelerometer found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the accel internal sampling rate up to at leat 500Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the accel internal sampling rate up to at leat 800Hz */ + ioctl(fd, ACCELIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system accel"); close(fd); } @@ -781,12 +796,27 @@ Sensors::gyro_init() errx(1, "FATAL: no gyro found"); } else { + + // XXX do the check more elegantly + + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* set the gyro internal sampling rate up to at leat 500Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 500); /* set the driver to poll at 500Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 500); + #else + + /* set the gyro internal sampling rate up to at leat 800Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 800); + + /* set the driver to poll at 800Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 800); + + #endif + warnx("using system gyro"); close(fd); } @@ -1387,6 +1417,9 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); + /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ + orb_set_interval(_gyro_sub, 4); + /* * do advertisements */ @@ -1422,7 +1455,7 @@ Sensors::task_main() while (!_task_should_exit) { - /* wait for up to 500ms for data */ + /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); /* timed out - periodic check for _task_should_exit, etc. */ -- cgit v1.2.3 From 74802f0692ad61ef3995b2f05c7af043ab9fcaf3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 17 Aug 2013 18:01:51 +0200 Subject: Added possibility to set board orientation --- src/modules/sensors/sensor_params.c | 3 + src/modules/sensors/sensors.cpp | 148 +++++++++++++++++++++++++++++++++--- 2 files changed, 141 insertions(+), 10 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 255dfed18..8d3992963 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b971..7299b21bc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include @@ -137,6 +138,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) +/** + * Enum for board and external compass rotations + * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + /** * Sensor app start / stop handling function * @@ -210,6 +282,9 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -227,6 +302,9 @@ private: float accel_scale[3]; float diff_pres_offset_pa; + int board_rotation; + int external_mag_rotation; + int rc_type; int rc_map_roll; @@ -306,6 +384,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -314,6 +395,11 @@ private: */ int parameters_update(); + /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + /** * Do accel-related initialisation. */ @@ -450,7 +536,10 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3) { /* basic r/c parameters */ @@ -540,6 +629,10 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -731,9 +824,33 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } +void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + void Sensors::accel_init() { @@ -874,9 +991,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) 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; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +1017,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) 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; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +1043,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw) 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; + // XXX TODO add support for external mag orientation + + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; -- cgit v1.2.3 From 628e806df5b05ea8a44d46f29606db03fee1fce9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:48:14 +0200 Subject: Minor style changes --- src/modules/sensors/sensors.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7299b21bc..c47f6bb7d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -139,8 +139,8 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** - * Enum for board and external compass rotations - * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. */ enum Rotation { ROTATION_NONE = 0, @@ -261,8 +261,8 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -282,8 +282,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ struct { float min[_rc_max_chan_count]; @@ -291,7 +291,6 @@ private: float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -343,7 +342,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; param_t rc_type; param_t rc_demix; @@ -874,7 +872,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -882,6 +880,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); -- cgit v1.2.3 From bc717f1715590a6915c223dde53fe6e1139f92d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:33:59 +0200 Subject: Sensors should now take into account the orientation of an external mag --- src/modules/sensors/sensors.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c47f6bb7d..198da9f0a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -284,6 +284,7 @@ private: math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ struct { float min[_rc_max_chan_count]; @@ -537,7 +538,8 @@ Sensors::Sensors() : _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _board_rotation(3,3), - _external_mag_rotation(3,3) + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -948,6 +950,14 @@ Sensors::mag_init() /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; + close(fd); } @@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - // XXX TODO add support for external mag orientation - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; - vect = _board_rotation*vect; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); -- cgit v1.2.3 From f4b5a17a7b6385869933cd195afd674fa532e735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:35:07 +0200 Subject: Improved sensor startup and error checking --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++++ src/modules/sensors/sensors.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) (limited to 'src/modules/sensors') diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b..4cfd59d54 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,10 @@ then hmc5883 start set BOARD fmuv1 else + if hmc5883 start + then + echo "Using external mag" + fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a6204c9fa..6e57a79a8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -953,7 +953,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index d1dd85d47..e7d9ce85f 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -263,7 +263,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 150; i++) + for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); -- cgit v1.2.3 From f5c92314f16fde650ee6f2f4fa20b7c2680a4b00 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 20 Aug 2013 20:02:06 +0200 Subject: Improved LSM303D driver, plus some fixes to the HMC5883 --- src/drivers/drv_mag.h | 25 ++- src/drivers/hmc5883/hmc5883.cpp | 5 + src/drivers/lsm303d/lsm303d.cpp | 482 +++++++++++++++++++++++----------------- src/modules/sensors/sensors.cpp | 25 ++- src/systemcmds/config/config.c | 71 ++++-- 5 files changed, 379 insertions(+), 229 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index e95034e8e..3de5625fd 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -90,28 +90,37 @@ ORB_DECLARE(sensor_mag); /** set the mag internal sample rate to at least (arg) Hz */ #define MAGIOCSSAMPLERATE _MAGIOC(0) +/** return the mag internal sample rate in Hz */ +#define MAGIOCGSAMPLERATE _MAGIOC(1) + /** set the mag internal lowpass filter to no lower than (arg) Hz */ -#define MAGIOCSLOWPASS _MAGIOC(1) +#define MAGIOCSLOWPASS _MAGIOC(2) + +/** return the mag internal lowpass filter in Hz */ +#define MAGIOCGLOWPASS _MAGIOC(3) /** set the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCSSCALE _MAGIOC(2) +#define MAGIOCSSCALE _MAGIOC(4) /** copy the mag scaling constants to the structure pointed to by (arg) */ -#define MAGIOCGSCALE _MAGIOC(3) +#define MAGIOCGSCALE _MAGIOC(5) /** set the measurement range to handle (at least) arg Gauss */ -#define MAGIOCSRANGE _MAGIOC(4) +#define MAGIOCSRANGE _MAGIOC(6) + +/** return the current mag measurement range in Gauss */ +#define MAGIOCGRANGE _MAGIOC(7) /** perform self-calibration, update scale factors to canonical units */ -#define MAGIOCCALIBRATE _MAGIOC(5) +#define MAGIOCCALIBRATE _MAGIOC(8) /** excite strap */ -#define MAGIOCEXSTRAP _MAGIOC(6) +#define MAGIOCEXSTRAP _MAGIOC(9) /** perform self test and report status */ -#define MAGIOCSELFTEST _MAGIOC(7) +#define MAGIOCSELFTEST _MAGIOC(10) /** determine if mag is external or onboard */ -#define MAGIOCGEXTERNAL _MAGIOC(8) +#define MAGIOCGEXTERNAL _MAGIOC(11) #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 44304fc22..1afc16a9a 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -637,13 +637,18 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; case MAGIOCSSAMPLERATE: + case MAGIOCGSAMPLERATE: /* not supported, always 1 sample per poll */ return -EINVAL; case MAGIOCSRANGE: return set_range(arg); + case MAGIOCGRANGE: + return _range_ga; + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: /* not supported, no internal filtering */ return -EINVAL; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e6ce64b8..8d6dc0672 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -180,56 +180,62 @@ public: LSM303D(int bus, const char* path, spi_dev_e device); virtual ~LSM303D(); - virtual int init(); + virtual int init(); virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** * Diagnostics - print some basic information about the driver. */ - void print_info(); + void print_info(); protected: - virtual int probe(); + virtual int probe(); friend class LSM303D_mag; virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg); private: - LSM303D_mag *_mag; + LSM303D_mag *_mag; struct hrt_call _accel_call; struct hrt_call _mag_call; - unsigned _call_accel_interval; - unsigned _call_mag_interval; + unsigned _call_accel_interval; + unsigned _call_mag_interval; - unsigned _num_accel_reports; + unsigned _num_accel_reports; volatile unsigned _next_accel_report; volatile unsigned _oldest_accel_report; struct accel_report *_accel_reports; - struct accel_scale _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - - unsigned _current_samplerate; - - unsigned _num_mag_reports; + unsigned _num_mag_reports; volatile unsigned _next_mag_report; volatile unsigned _oldest_mag_report; struct mag_report *_mag_reports; + struct accel_scale _accel_scale; + unsigned _accel_range_g; + float _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_filter_bandwith; + struct mag_scale _mag_scale; - float _mag_range_scale; - float _mag_range_ga; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; orb_advert_t _mag_topic; + unsigned _accel_read; + unsigned _mag_read; + perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; @@ -240,12 +246,19 @@ private: /** * Start automatic measurement. */ - void start(); + void start(); /** * Stop automatic measurement. */ - void stop(); + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + void reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -256,24 +269,38 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void measure_trampoline(void *arg); + static void measure_trampoline(void *arg); /** * Static trampoline for the mag because it runs at a lower rate * * @param arg Instance pointer for the driver that is polling. */ - static void mag_measure_trampoline(void *arg); + static void mag_measure_trampoline(void *arg); /** * Fetch accel measurements from the sensor and update the report ring. */ - void measure(); + void measure(); /** * Fetch mag measurements from the sensor and update the report ring. */ - void mag_measure(); + void mag_measure(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); /** * Read a register from the LSM303D @@ -281,7 +308,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg); /** * Write a register in the LSM303D @@ -289,7 +316,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(unsigned reg, uint8_t value); /** * Modify a register in the LSM303D @@ -300,7 +327,7 @@ private: * @param clearbits Bits in the register to clear. * @param setbits Bits in the register to set. */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); /** * Set the LSM303D accel measurement range. @@ -309,7 +336,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_range(unsigned max_g); + int accel_set_range(unsigned max_g); /** * Set the LSM303D mag measurement range. @@ -318,7 +345,7 @@ private: * Zero selects the maximum supported range. * @return OK if the value can be supported, -ERANGE otherwise. */ - int mag_set_range(unsigned max_g); + int mag_set_range(unsigned max_g); /** * Set the LSM303D accel anti-alias filter. @@ -327,15 +354,7 @@ private: * Zero selects the highest bandwidth * @return OK if the value can be supported, -ERANGE otherwise. */ - int set_antialias_filter_bandwidth(unsigned bandwith); - - /** - * Get the LSM303D accel anti-alias filter. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * @return OK if the value was read and supported, ERROR otherwise. - */ - int get_antialias_filter_bandwidth(unsigned &bandwidth); + int accel_set_antialias_filter_bandwidth(unsigned bandwith); /** * Set the LSM303D internal accel sampling frequency. @@ -345,7 +364,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int set_samplerate(unsigned frequency); + int accel_set_samplerate(unsigned frequency); /** * Set the LSM303D internal mag sampling frequency. @@ -355,7 +374,7 @@ private: * Zero selects the maximum rate supported. * @return OK if the value can be supported. */ - int mag_set_samplerate(unsigned frequency); + int mag_set_samplerate(unsigned frequency); }; /** @@ -396,16 +415,22 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _next_accel_report(0), _oldest_accel_report(0), _accel_reports(nullptr), - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(-1), - _current_samplerate(0), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), + _accel_range_g(8), + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(800), + _accel_filter_bandwith(50), + _mag_range_ga(2), _mag_range_scale(0.0f), - _mag_range_ga(0.0f), + _mag_samplerate(100), + _accel_topic(-1), + _mag_topic(-1), + _accel_read(0), + _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_filter_x(800, 30), @@ -416,18 +441,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _debug_enabled = true; // default scale factors - _accel_scale.x_offset = 0; + _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; + _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; + _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; - _mag_scale.x_offset = 0; + _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; + _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; + _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; } @@ -478,27 +503,12 @@ LSM303D::init() if (_mag_reports == nullptr) goto out; + reset(); + /* advertise mag topic */ memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); - /* enable accel, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); - - /* enable mag, XXX do this with an ioctl? */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - - /* XXX should we enable FIFO? */ - - set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */ - set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */ - set_samplerate(400); /* max sample rate */ - - mag_set_range(4); /* XXX: take highest sensor range of 12GA? */ - mag_set_samplerate(100); - - /* XXX test this when another mag is used */ /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -511,6 +521,24 @@ out: return ret; } +void +LSM303D::reset() +{ + /* enable accel*/ + write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + + /* enable mag */ + write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + + accel_set_range(_accel_range_g); + accel_set_samplerate(_accel_samplerate); + accel_set_antialias_filter_bandwidth(_accel_filter_bandwith); + + mag_set_range(_mag_range_ga); + mag_set_samplerate(_mag_samplerate); +} + int LSM303D::probe() { @@ -612,64 +640,60 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_accel_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1600); case SENSOR_POLLRATE_DEFAULT: - /* With internal low pass filters enabled, 250 Hz is sufficient */ - /* XXX for vibration tests with 800 Hz */ + /* Use 800Hz as it is filtered in the driver as well*/ return ioctl(filp, SENSORIOCSPOLLRATE, 800); /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 1000) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) + return -EINVAL; - /* adjust sample rate of sensor */ - set_samplerate(arg); + /* adjust filters */ + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - // adjust filters - float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_x.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency((float)arg, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency((float)arg, cutoff_freq_hz); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); - return OK; - } + return OK; } } + } case SENSORIOCGPOLLRATE: if (_call_accel_interval == 0) @@ -678,66 +702,78 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; + /* account for sentinel in the ring */ + arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; - if (nullptr == buf) - return -ENOMEM; + if (nullptr == buf) + return -ENOMEM; - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _num_accel_reports - 1; case SENSORIOCRESET: - /* XXX implement */ - return -EINVAL; + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; case ACCELIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_accel_interval; - _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); - return OK; - } + _accel_filter_x.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_y.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + _accel_filter_z.set_cutoff_frequency((float)_accel_samplerate, (float)arg); + return OK; + } case ACCELIOCGLOWPASS: - return _accel_filter_x.get_cutoff_freq(); - - case ACCELIOCSSCALE: - { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; - } + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + } else { + return -EINVAL; } + } + + case ACCELIOCSRANGE: + return accel_set_range(arg); + + case ACCELIOCGRANGE: + return _accel_range_g; case ACCELIOCGSCALE: /* copy scale out */ memcpy((struct accel_scale *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; + case ACCELIOCSELFTEST: + return accel_self_test(); + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -768,7 +804,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: - /* 50 Hz is max for mag */ + /* 100 Hz is max for mag */ return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); /* adjust to a legal polling interval in Hz */ @@ -783,9 +819,6 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) if (ticks < 1000) return -EINVAL; - /* adjust sample rate of sensor */ - mag_set_samplerate(arg); - /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; @@ -833,17 +866,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return _num_mag_reports - 1; case SENSORIOCRESET: - return ioctl(filp, cmd, arg); + reset(); + return OK; case MAGIOCSSAMPLERATE: -// case MAGIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; case MAGIOCSLOWPASS: -// case MAGIOCGLOWPASS: - /* XXX not implemented */ -// _set_dlpf_filter((uint16_t)arg); + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ return -EINVAL; case MAGIOCSSCALE: @@ -857,17 +891,13 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case MAGIOCSRANGE: -// case MAGIOCGRANGE: - /* XXX not implemented */ - // XXX change these two values on set: - // _mag_range_scale = xx - // _mag_range_ga = xx - return -EINVAL; + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; case MAGIOCSELFTEST: - /* XXX not implemented */ -// return self_test(); - return -EINVAL; + return mag_self_test(); case MAGIOCGEXTERNAL: /* no external mag board yet */ @@ -879,6 +909,53 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } } +int +LSM303D::accel_self_test() +{ + if (_accel_read == 0) + return 1; + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.y_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + return 1; + + if (fabsf(_accel_scale.z_offset) < 0.000001f) + return 1; + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + return 1; + + return 0; +} + +int +LSM303D::mag_self_test() +{ + if (_mag_read == 0) + return 1; + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.y_offset) < 0.000001f) + return 1; + + if (fabsf(_mag_scale.z_offset) < 0.000001f) + return 1; + + return 0; +} + uint8_t LSM303D::read_reg(unsigned reg) { @@ -914,38 +991,37 @@ LSM303D::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) } int -LSM303D::set_range(unsigned max_g) +LSM303D::accel_set_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = REG2_FULL_SCALE_BITS_A; - float new_range_g = 0.0f; float new_scale_g_digit = 0.0f; if (max_g == 0) max_g = 16; if (max_g <= 2) { - new_range_g = 2.0f; + _accel_range_g = 2; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - new_range_g = 4.0f; + _accel_range_g = 4; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - new_range_g = 6.0f; + _accel_range_g = 6; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - new_range_g = 8.0f; + _accel_range_g = 8; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - new_range_g = 16.0f; + _accel_range_g = 16; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -953,7 +1029,7 @@ LSM303D::set_range(unsigned max_g) return -EINVAL; } - _accel_range_m_s2 = new_range_g * 9.80665f; + _accel_range_m_s2 = _accel_range_g * 9.80665f; _accel_range_scale = new_scale_g_digit * 9.80665f; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); @@ -966,29 +1042,28 @@ LSM303D::mag_set_range(unsigned max_ga) { uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; - float new_range = 0.0f; float new_scale_ga_digit = 0.0f; if (max_ga == 0) max_ga = 12; if (max_ga <= 2) { - new_range = 2.0f; + _mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080e-3f; } else if (max_ga <= 4) { - new_range = 4.0f; + _mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160e-3f; } else if (max_ga <= 8) { - new_range = 8.0f; + _mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320e-3f; } else if (max_ga <= 12) { - new_range = 12.0f; + _mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479e-3f; @@ -996,7 +1071,6 @@ LSM303D::mag_set_range(unsigned max_ga) return -EINVAL; } - _mag_range_ga = new_range; _mag_range_scale = new_scale_ga_digit; modify_reg(ADDR_CTRL_REG6, clearbits, setbits); @@ -1005,7 +1079,7 @@ LSM303D::mag_set_range(unsigned max_ga) } int -LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) +LSM303D::accel_set_antialias_filter_bandwidth(unsigned bandwidth) { uint8_t setbits = 0; uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; @@ -1015,15 +1089,19 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) if (bandwidth <= 50) { setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_filter_bandwith = 50; } else if (bandwidth <= 194) { setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_filter_bandwith = 194; } else if (bandwidth <= 362) { setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_filter_bandwith = 362; } else if (bandwidth <= 773) { setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_filter_bandwith = 773; } else { return -EINVAL; @@ -1035,26 +1113,7 @@ LSM303D::set_antialias_filter_bandwidth(unsigned bandwidth) } int -LSM303D::get_antialias_filter_bandwidth(unsigned &bandwidth) -{ - uint8_t readbits = read_reg(ADDR_CTRL_REG2); - - if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_50HZ_A) - bandwidth = 50; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_194HZ_A) - bandwidth = 194; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_362HZ_A) - bandwidth = 362; - else if ((readbits & REG2_ANTIALIAS_FILTER_BW_BITS_A) == REG2_AA_FILTER_BW_773HZ_A) - bandwidth = 773; - else - return ERROR; - - return OK; -} - -int -LSM303D::set_samplerate(unsigned frequency) +LSM303D::accel_set_samplerate(unsigned frequency) { uint8_t setbits = 0; uint8_t clearbits = REG1_RATE_BITS_A; @@ -1064,23 +1123,23 @@ LSM303D::set_samplerate(unsigned frequency) if (frequency <= 100) { setbits |= REG1_RATE_100HZ_A; - _current_samplerate = 100; + _accel_samplerate = 100; } else if (frequency <= 200) { setbits |= REG1_RATE_200HZ_A; - _current_samplerate = 200; + _accel_samplerate = 200; } else if (frequency <= 400) { setbits |= REG1_RATE_400HZ_A; - _current_samplerate = 400; + _accel_samplerate = 400; } else if (frequency <= 800) { setbits |= REG1_RATE_800HZ_A; - _current_samplerate = 800; + _accel_samplerate = 800; } else if (frequency <= 1600) { setbits |= REG1_RATE_1600HZ_A; - _current_samplerate = 1600; + _accel_samplerate = 1600; } else { return -EINVAL; @@ -1102,13 +1161,15 @@ LSM303D::mag_set_samplerate(unsigned frequency) if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; + _mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; + _mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; - + _mag_samplerate = 100; } else { return -EINVAL; @@ -1229,6 +1290,8 @@ LSM303D::measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + _accel_read++; + /* stop the perf counter */ perf_end(_accel_sample_perf); } @@ -1281,7 +1344,7 @@ LSM303D::mag_measure() mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; mag_report->scaling = _mag_range_scale; - mag_report->range_ga = _mag_range_ga; + mag_report->range_ga = (float)_mag_range_ga; /* post a report to the ring - note, not locked */ INCREMENT(_next_mag_report, _num_mag_reports); @@ -1297,6 +1360,8 @@ LSM303D::mag_measure() /* publish for subscribers */ orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + _mag_read++; + /* stop the perf counter */ perf_end(_mag_sample_perf); } @@ -1304,6 +1369,8 @@ LSM303D::mag_measure() void LSM303D::print_info() { + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); printf("report queue: %u (%u/%u @ %p)\n", _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); @@ -1466,7 +1533,7 @@ test() /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) errx(1, "failed to get if mag is onboard or external"); - warnx("device active: %s", ret ? "external" : "onboard"); + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); @@ -1484,7 +1551,7 @@ test() /* XXX add poll-rate tests here too */ -// reset(); + reset(); errx(0, "PASS"); } @@ -1503,7 +1570,16 @@ reset() err(1, "driver reset failed"); if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) - err(1, "driver poll restart failed"); + err(1, "accel driver poll rate reset failed"); + + int fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd_mag < 0) { + warnx("could not open to mag " MAG_DEVICE_PATH); + } else { + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "mag driver poll rate reset failed"); + } exit(0); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 198da9f0a..7ea1ae0f3 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -936,6 +936,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -944,13 +945,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); +#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } +#endif + - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 766598ddd..262a52d20 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -90,6 +90,7 @@ static void do_gyro(int argc, char *argv[]) { int fd; + int ret; fd = open(GYRO_DEVICE_PATH, 0); @@ -102,20 +103,29 @@ do_gyro(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the gyro internal sampling rate up to at least i Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { /* set the range to i dps */ - ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, GYROIOCSELFTEST, 0); + ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { warnx("gyro self test FAILED! Check calibration:"); @@ -147,6 +157,7 @@ static void do_mag(int argc, char *argv[]) { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -156,8 +167,32 @@ do_mag(int argc, char *argv[]) } else { - if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, MAGIOCSELFTEST, 0); + if (argc == 2 && !strcmp(argv[0], "sampling")) { + + /* set the mag internal sampling rate up to at least i Hz */ + ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "rate")) { + + /* set the driver to poll at i Hz */ + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); + + } else if (argc == 2 && !strcmp(argv[0], "range")) { + + /* set the range to i G */ + ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); + + } else if(argc == 1 && !strcmp(argv[0], "check")) { + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret) { warnx("mag self test FAILED! Check calibration:"); @@ -173,9 +208,9 @@ do_mag(int argc, char *argv[]) errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); } - int srate = -1; //ioctl(fd, MAGIOCGSAMPLERATE, 0); + int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); - int range = -1; //ioctl(fd, MAGIOCGRANGE, 0); + int range = ioctl(fd, MAGIOCGRANGE, 0); warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d gauss", srate, prate, range); @@ -189,6 +224,7 @@ static void do_accel(int argc, char *argv[]) { int fd; + int ret; fd = open(ACCEL_DEVICE_PATH, 0); @@ -201,20 +237,29 @@ do_accel(int argc, char *argv[]) if (argc == 2 && !strcmp(argv[0], "sampling")) { /* set the accel internal sampling rate up to at least i Hz */ - ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"sampling rate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "rate")) { /* set the driver to poll at i Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"pollrate could not be set"); } else if (argc == 2 && !strcmp(argv[0], "range")) { - /* set the range to i m/s^2 */ - ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + /* set the range to i G */ + ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[1], NULL, 0)); + + if (ret) + errx(ret,"range could not be set"); } else if(argc == 1 && !strcmp(argv[0], "check")) { - int ret = ioctl(fd, ACCELIOCSELFTEST, 0); + ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret) { warnx("accel self test FAILED! Check calibration:"); -- cgit v1.2.3 From 9762cf86a081f44e7f7ea48f160d556003bf5be9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 09:52:21 +0200 Subject: Forgot to comment mag init in sensors.cpp back back in --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 7ea1ae0f3..dda558b4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -947,7 +947,7 @@ Sensors::mag_init() /* try different mag sampling rates */ -#if 0 + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); if (ret == OK) { /* set the pollrate accordingly */ @@ -962,7 +962,7 @@ Sensors::mag_init() errx(1, "FATAL: mag sampling rate could not be set"); } } -#endif + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); -- cgit v1.2.3 From 8083efb60c97ffce5be8dcbf3956ab67cc17d729 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 21 Aug 2013 14:21:11 +0200 Subject: Use gyro at correct rate --- src/modules/sensors/sensors.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index dda558b4c..2ffa5f698 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -919,11 +919,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif -- cgit v1.2.3 From bb91484b2648800923a135be28924119a1382ba6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 17:34:59 +0200 Subject: Default flight mode switches parameters changed. --- src/modules/sensors/sensor_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2bd869263..8c2beb456 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,8 +167,8 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); -- cgit v1.2.3 From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +----- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 +++++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++++ src/systemcmds/preflight_check/preflight_check.c | 97 +------------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h (limited to 'src/modules/sensors') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..30edf679a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e9258..82df7c37d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..e3d314881 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a15..42f0190c7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbe..fd2a6318e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c465..e857b1c2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c03..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..9d47d8000 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e2238d151 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85f..4c19dcffb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; -- cgit v1.2.3 From 0fb3be64eace32c9be8a17dab3d1cc3ee0459f9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 11:18:10 +0200 Subject: More verbosity on RC cal fail in sensors app --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e857b1c2f..a9c49c441 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -673,7 +673,7 @@ Sensors::parameters_update() if (!isfinite(_parameters.scaling_factor[i]) || _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0; rc_valid = false; -- cgit v1.2.3 From b7ee1d34294b536c6a92c8e72455f24684a5db4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Aug 2013 14:34:49 +0200 Subject: Prevented an analog airspeed corner case from happening --- src/modules/sensors/sensor_params.c | 3 ++- src/modules/sensors/sensors.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fd2a6318e..16bfc8094 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9c49c441..88a6bebb8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -301,6 +301,7 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; int board_rotation; int external_mag_rotation; @@ -348,6 +349,7 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; @@ -617,6 +619,7 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); @@ -784,6 +787,7 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { @@ -1266,9 +1270,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor -- cgit v1.2.3 From b986607a750db74e05c60d09b048971bb0cfb18f Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 18:32:16 -0700 Subject: Add defines for RC15 params (16 channels total) minor cleanup of rc sanity check code --- src/modules/sensors/sensor_params.c | 7 +++++++ src/modules/sensors/sensors.cpp | 23 ++++++++++++++--------- 2 files changed, 21 insertions(+), 9 deletions(-) (limited to 'src/modules/sensors') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 16bfc8094..dd86f3e2e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -158,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 88a6bebb8..e98c4d548 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -660,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -670,18 +672,21 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, _parameters.scaling_factor[i], (int)(_parameters.rev[i])); + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ -- cgit v1.2.3