From 9eea4f79d9588153b751b7ba6053840c94b89194 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:03 +0400 Subject: sensors: support for Futaba RC failsafe --- src/modules/sensors/sensor_params.c | 4 ++++ src/modules/sensors/sensors.cpp | 27 +++++++++++++++++++++++++++ 2 files changed, 31 insertions(+) (limited to 'src') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 587b81588..2aa15420a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -226,3 +226,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); + +PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372..21da44d10 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -260,6 +260,10 @@ private: float rc_scale_yaw; float rc_scale_flaps; + int rc_fs_ch; + int rc_fs_mode; + float rc_fs_thr; + float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -305,6 +309,10 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; + param_t rc_fs_ch; + param_t rc_fs_mode; + param_t rc_fs_thr; + param_t battery_voltage_scaling; param_t board_rotation; @@ -517,6 +525,11 @@ Sensors::Sensors() : _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); + /* RC failsafe */ + _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); + _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); + _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -668,6 +681,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); + param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); + param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); + param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -1255,6 +1271,17 @@ Sensors::rc_poll() if (rc_input.channel_count < 4) return; + /* failsafe check */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) + return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) + return; + } + } + unsigned channel_limit = rc_input.channel_count; if (channel_limit > _rc_max_chan_count) -- cgit v1.2.3 From 97acb49028da526ad9081e9eecea97ec0a822858 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:58:23 +0400 Subject: commander: bug fixed in failsafe --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271c..cbf900fce 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1600,8 +1600,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c /* switch to failsafe mode */ bool manual_control_old = control_mode->flag_control_manual_enabled; - if (!status->condition_landed) { - /* in air: try to hold position */ + if (!status->condition_landed && status->condition_local_position_valid) { + /* in air: try to hold position if possible */ res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); } else { -- cgit v1.2.3 From af12065826060e3ac071869bf39a82695e1d88e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 1 Nov 2013 13:59:24 +0400 Subject: sensors: code style fixed --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 21da44d10..3b1460b73 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,7 @@ Sensors::rc_poll() if (_parameters.rc_fs_mode == 0) { if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) return; + } else if (_parameters.rc_fs_mode == 1) { if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) return; -- cgit v1.2.3 From e46d60ba6de3c3809cd7e1e8e1f0485f0290980b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 15 Nov 2013 11:32:05 +0400 Subject: px4io driver: don’t use PX4IO_PAGE_ACTUATORS page for actuator_controls_effective MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/drivers/px4io/px4io.cpp | 25 ++++++++++++++++++------- src/modules/px4iofirmware/mixer.cpp | 6 +++++- 2 files changed, 23 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f..1ab18fe7b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1378,18 +1378,29 @@ PX4IO::io_publish_mixed_controls() actuator_controls_effective_s controls_effective; controls_effective.timestamp = hrt_absolute_time(); - /* get actuator controls from IO */ - uint16_t act[_max_actuators]; - int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + // XXX PX4IO_PAGE_ACTUATORS page contains actuator outputs, not inputs + // need to implement backward mixing in IO's mixed and add another page + // by now simply use control inputs here + /* get actuator outputs from IO */ + //uint16_t act[_max_actuators]; + //int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); - if (ret != OK) - return ret; + //if (ret != OK) + // return ret; /* convert from register format to float */ + //for (unsigned i = 0; i < _max_actuators; i++) + // controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + + actuator_controls_s controls; + + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &controls); + for (unsigned i = 0; i < _max_actuators; i++) - controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]); + controls_effective.control_effective[i] = controls.control[i]; - /* laxily advertise on first publication */ + /* lazily advertise on first publication */ if (_to_actuators_effective == 0) { _to_actuators_effective = orb_advertise((_primary_pwm_device ? diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 05897b4ce..35ef5fcf6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -185,7 +185,7 @@ mixer_tick(void) r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ - r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f; + r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f); } @@ -201,6 +201,10 @@ mixer_tick(void) for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { + r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); + } } if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { -- cgit v1.2.3 From 6ed268aa28dead90d3b78884ecda44df41d32188 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 15 Nov 2013 11:42:19 +0400 Subject: mavlink: some mavling messages filling bugs fixed --- src/modules/mavlink/orb_listener.c | 47 ++++++++++++-------------------------- 1 file changed, 15 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34f..46460d3b8 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -248,8 +249,8 @@ l_vehicle_attitude(const struct listener *l) if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; - float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; + float throttle = actuators_effective_0.control_effective[3] * 100.0f; mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } } @@ -266,13 +267,7 @@ l_vehicle_gps_position(const struct listener *l) orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = gps.cog_rad; - - if (cog_deg > M_PI_F) - cog_deg -= 2.0f * M_PI_F; - - cog_deg *= M_RAD_TO_DEG_F; - + float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, @@ -365,28 +360,16 @@ l_global_position(const struct listener *l) /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - uint64_t timestamp = global_pos.timestamp; - int32_t lat = global_pos.lat; - int32_t lon = global_pos.lon; - int32_t alt = (int32_t)(global_pos.alt * 1000); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); - int16_t vx = (int16_t)(global_pos.vx * 100.0f); - int16_t vy = (int16_t)(global_pos.vy * 100.0f); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, - timestamp / 1000, - lat, - lon, - alt, - relative_alt, - vx, - vy, - vz, - hdg); + global_pos.timestamp / 1000, + global_pos.lat, + global_pos.lon, + global_pos.alt * 1000.0f, + global_pos.relative_alt * 1000.0f, + global_pos.vx * 100.0f, + global_pos.vy * 100.0f, + global_pos.vz * 100.0f, + _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void @@ -424,8 +407,8 @@ l_global_position_setpoint(const struct listener *l) coordinate_frame, global_sp.lat, global_sp.lon, - global_sp.altitude, - global_sp.yaw); + global_sp.altitude * 1000.0f, + global_sp.yaw * M_RAD_TO_DEG_F * 100.0f); } void -- cgit v1.2.3 From 45e158b88c1b4dec802c419265d656e706dbe5e6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:06:23 +0400 Subject: Fixed actuator_controls_effective on FMU --- src/drivers/px4fmu/fmu.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0441566e9..6ccb8acdb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -542,7 +542,7 @@ PX4FMU::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { @@ -589,8 +589,9 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output actual limited values */ - for (unsigned i = 0; i < num_outputs; i++) { - controls_effective.control_effective[i] = (float)pwm_limited[i]; + // XXX copy control values as is, need to do backward mixing of actual limited values properly + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + controls_effective.control_effective[i] = _controls.control[i]; } orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); -- cgit v1.2.3 From 8f559c73e9f3ed2f44aba5fe4fdfbae2542ad8bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:07:06 +0400 Subject: px4io driver: bug fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1ab18fe7b..2b8a65ae6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2541,7 +2541,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[4]; /* work out what we're uploading... */ if (argc > 2) { -- cgit v1.2.3 From 63d81ba415302c3ed62b4928e6977b4a5da6767b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 23:16:09 +0400 Subject: actuator_controls_effective topic removed --- .../ardrone_interface/ardrone_motor_control.c | 23 -------- src/drivers/mkblctrl/mkblctrl.cpp | 14 ----- src/drivers/px4fmu/fmu.cpp | 18 ------ src/drivers/px4io/px4io.cpp | 67 +--------------------- src/modules/mavlink/orb_listener.c | 34 +---------- src/modules/sdlog2/sdlog2.c | 21 +------ src/modules/uORB/objects_common.cpp | 7 --- .../uORB/topics/actuator_controls_effective.h | 60 +++++++++---------- 8 files changed, 35 insertions(+), 209 deletions(-) (limited to 'src') diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 01b89c8fa..81f634992 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -46,7 +46,6 @@ #include #include #include -#include #include #include "ardrone_motor_control.h" @@ -384,9 +383,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ static bool initialized = false; - /* publish effective outputs */ - static struct actuator_controls_effective_s actuator_controls_effective; - static orb_advert_t actuator_controls_effective_pub; /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { @@ -430,25 +426,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); } - - - /* publish effective outputs */ - actuator_controls_effective.control_effective[0] = roll_control; - actuator_controls_effective.control_effective[1] = pitch_control; - /* yaw output after limiting */ - actuator_controls_effective.control_effective[2] = yaw_control; - /* possible motor thrust limiting */ - actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f; - - if (!initialized) { - /* advertise and publish */ - actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective); - initialized = true; - } else { - /* already initialized, just publishing */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective); - } - /* set the motor values */ /* scale up from 0..1 to 10..500) */ diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6..7ef883f94 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -73,7 +73,6 @@ #include #include -#include #include #include #include @@ -143,7 +142,6 @@ private: int _frametype; orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -252,7 +250,6 @@ MK::MK(int bus) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -525,13 +522,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -595,9 +585,6 @@ MK::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -701,7 +688,6 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 6ccb8acdb..39112ab1e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -123,7 +122,6 @@ private: int _t_actuators; int _t_actuator_armed; orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -219,7 +217,6 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -466,13 +463,6 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -588,13 +578,6 @@ PX4FMU::task_main() pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - /* output actual limited values */ - // XXX copy control values as is, need to do backward mixing of actual limited values properly - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - controls_effective.control_effective[i] = _controls.control[i]; - } - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { up_pwm_servo_set(i, pwm_limited[i]); @@ -651,7 +634,6 @@ PX4FMU::task_main() } ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2b8a65ae6..0ed30ed91 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,7 +72,6 @@ #include #include -#include #include #include #include @@ -257,14 +256,12 @@ private: /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io - orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; /// #include #include -#include #include #include #include @@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; - struct actuator_controls_effective_s act_controls_effective; struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; @@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int rates_sp_sub; int act_outputs_sub; int act_controls_sub; - int act_controls_effective_sub; int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; @@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 20; - /* Sanity check variable and index */ + /* number of subscriptions */ + const ssize_t fdsc = 19; + /* sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); - fds[fdsc_count].fd = subs.act_controls_effective_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); fds[fdsc_count].fd = subs.local_pos_sub; @@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(ATTC); } - /* --- ACTUATOR CONTROL EFFECTIVE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective); - // TODO not implemented yet - } - /* --- LOCAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3514dca24..c6a252b55 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); #include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); -/* actuator controls, as set by actuators / mixers after limiting */ -#include "topics/actuator_controls_effective.h" -ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s); -ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s); - #include "topics/actuator_outputs.h" ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h index d7b404ad4..54d84231f 100644 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ b/src/modules/uORB/topics/actuator_controls_effective.h @@ -46,34 +46,34 @@ #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#include -#include "../uORB.h" -#include "actuator_controls.h" +//#include +//#include "../uORB.h" +//#include "actuator_controls.h" +// +//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ +// +///** +// * @addtogroup topics +// * @{ +// */ +// +//struct actuator_controls_effective_s { +// uint64_t timestamp; +// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; +//}; +// +///** +// * @} +// */ +// +///* actuator control sets; this list can be expanded as more controllers emerge */ +//ORB_DECLARE(actuator_controls_effective_0); +//ORB_DECLARE(actuator_controls_effective_1); +//ORB_DECLARE(actuator_controls_effective_2); +//ORB_DECLARE(actuator_controls_effective_3); +// +///* control sets with pre-defined applications */ +//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) -#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ - -/** - * @addtogroup topics - * @{ - */ - -struct actuator_controls_effective_s { - uint64_t timestamp; - float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -}; - -/** - * @} - */ - -/* actuator control sets; this list can be expanded as more controllers emerge */ -ORB_DECLARE(actuator_controls_effective_0); -ORB_DECLARE(actuator_controls_effective_1); -ORB_DECLARE(actuator_controls_effective_2); -ORB_DECLARE(actuator_controls_effective_3); - -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ \ No newline at end of file +#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ -- cgit v1.2.3 From 21cc19cef6a6ad9d88ac20cf2223635fe8ec4388 Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 18 Nov 2013 21:32:41 +0100 Subject: mkblctrl set a default device / -d (device) parameter for alternate device added / -t testmode enhanced --- .../init.d/rc.custom_dji_f330_mkblctrl | 4 +- src/drivers/mkblctrl/mkblctrl.cpp | 73 +++++++++++++++------- 2 files changed, 52 insertions(+), 25 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl index 8e0914d63..40b2ee68b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -97,9 +97,9 @@ fi # if [ $MKBLCTRL_FRAME == x ] then - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix else - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix fi # diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index b93f38cf6..4e26d9c50 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * Author: Marco Bauer * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +36,8 @@ * @file mkblctrl.cpp * * Driver/configurator for the Mikrokopter BL-Ctrl. - * Marco Bauer + * + * @author Marco Bauer * */ @@ -89,8 +91,8 @@ #define BLCTRL_MIN_VALUE -0.920F #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F -#define MOTOR_SPINUP_COUNTER 2500 -#define ESC_UORB_PUBLISH_DELAY 200 +#define MOTOR_SPINUP_COUNTER 30 +#define ESC_UORB_PUBLISH_DELAY 200 class MK : public device::I2C { @@ -112,7 +114,7 @@ public: FRAME_X, }; - MK(int bus); + MK(int bus, const char *_device_path); ~MK(); virtual int ioctl(file *filp, int cmd, unsigned long arg); @@ -141,6 +143,7 @@ private: unsigned int _motor; int _px4mode; int _frametype; + char _device[20]; ///< device orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; @@ -244,7 +247,7 @@ MK *g_mk; } // namespace -MK::MK(int bus) : +MK::MK(int bus, const char *_device_path) : I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED), _mode(MODE_NONE), _update_rate(50), @@ -265,6 +268,10 @@ MK::MK(int bus) : _armed(false), _mixers(nullptr) { + strncpy(_device, _device_path, sizeof(_device)); + /* enforce null termination */ + _device[sizeof(_device) - 1] = '\0'; + _debug_enabled = true; } @@ -291,7 +298,7 @@ MK::~MK() /* clean up the alternate device node */ if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + unregister_driver(_device); g_mk = nullptr; } @@ -313,13 +320,15 @@ MK::init(unsigned motors) usleep(500000); - /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); + if (sizeof(_device) > 0) { + ret = register_driver(_device, &fops, 0666, (void *)this); - if (ret == OK) { - log("default PWM output device"); - _primary_pwm_device = true; - } + if (ret == OK) { + log("creating alternate output device"); + _primary_pwm_device = true; + } + + } /* reset GPIOs */ gpio_reset(); @@ -633,10 +642,7 @@ MK::task_main() } /* output to BLCtrl's */ - if (_motortest == true) { - mk_servo_test(i); - - } else { + if (_motortest != true) { //mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024)); // scale the output to 0 - 1024 and sent to output routine // 11 Bit Motor[i].SetPoint_PX4 = outputs.output[i]; @@ -692,9 +698,16 @@ MK::task_main() esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature; esc.esc[i].esc_state = (uint16_t) Motor[i].State; esc.esc[i].esc_errorcount = (uint16_t) 0; + + // if motortest is requested - do it... + if (_motortest == true) { + mk_servo_test(i); + } + } orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); + } } @@ -1390,13 +1403,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, } int -mk_start(unsigned bus, unsigned motors) +mk_start(unsigned bus, unsigned motors, char *device_path) { int ret = OK; if (g_mk == nullptr) { - g_mk = new MK(bus); + g_mk = new MK(bus, device_path); if (g_mk == nullptr) { ret = -ENOMEM; @@ -1432,6 +1445,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; + char *devicepath = ""; /* * optional parameters @@ -1491,25 +1505,38 @@ mkblctrl_main(int argc, char *argv[]) newMode = true; } + /* look for the optional device parameter */ + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + devicepath = argv[i + 1]; + newMode = true; + + } else { + errx(1, "missing the devicename (-d)"); + return 1; + } + } + } if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); - fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); + fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); + fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); exit(1); } if (g_mk == nullptr) { - if (mk_start(bus, motorcount) != OK) { + if (mk_start(bus, motorcount, devicepath) != OK) { errx(1, "failed to start the MK-BLCtrl driver"); } else { - newMode = true; + //////newMode = true; } } @@ -1522,5 +1549,5 @@ mkblctrl_main(int argc, char *argv[]) /* test, etc. here g*/ - exit(1); + exit(0); } -- cgit v1.2.3 From 3527ea8a62e4b4d896cd35bbe9e9c54b0edc1579 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Nov 2013 16:37:48 +0100 Subject: tecs: fix wrong != 0 check --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 864a9d24d..a733ef1d2 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time - if (fabsf(_throttle_slewrate) < 0.01f) { + if (fabsf(_throttle_slewrate) > 0.01f) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; _throttle_dem = constrain(_throttle_dem, -- cgit v1.2.3 From f82a202667c8b4e38d229e6fcac70ca40380aea2 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 19 Nov 2013 17:35:04 +0100 Subject: actuator effective removed - unused --- src/drivers/mkblctrl/mkblctrl.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 4e26d9c50..9442ae906 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -75,7 +75,6 @@ #include #include -#include #include #include #include @@ -146,7 +145,6 @@ private: char _device[20]; ///< device orb_advert_t _t_outputs; - orb_advert_t _t_actuators_effective; orb_advert_t _t_esc_status; unsigned int _num_outputs; @@ -255,7 +253,6 @@ MK::MK(int bus, const char *_device_path) : _t_actuators(-1), _t_actuator_armed(-1), _t_outputs(0), - _t_actuators_effective(0), _t_esc_status(0), _num_outputs(0), _motortest(false), @@ -534,13 +531,6 @@ MK::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); - /* advertise the effective control inputs */ - actuator_controls_effective_s controls_effective; - memset(&controls_effective, 0, sizeof(controls_effective)); - /* advertise the effective control inputs */ - _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), - &controls_effective); - /* advertise the blctrl status */ esc_status_s esc; memset(&esc, 0, sizeof(esc)); @@ -604,9 +594,6 @@ MK::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); outputs.timestamp = hrt_absolute_time(); - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - /* iterate actuators */ for (unsigned int i = 0; i < _num_outputs; i++) { @@ -712,9 +699,8 @@ MK::task_main() } - //::close(_t_esc_status); + ::close(_t_esc_status); ::close(_t_actuators); - ::close(_t_actuators_effective); ::close(_t_actuator_armed); -- cgit v1.2.3 From ee985c70b3d774f30d0fbe86f8c7046e49a74104 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Wed, 20 Nov 2013 02:45:52 +0900 Subject: Update fw_att_control_params.c Minor comment error corrected. --- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 97aa275de..2360c43af 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file fw_pos_control_l1_params.c + * @file fw_att_control_params.c * * Parameters defined by the L1 position control task * -- cgit v1.2.3 From 9a4b57c352cd11dfd448444d6754c60bd289f5e7 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Wed, 20 Nov 2013 03:04:53 +0900 Subject: Update fw_att_control_params.c --- src/modules/fw_att_control/fw_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 2360c43af..be76524da 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -35,7 +35,7 @@ /** * @file fw_att_control_params.c * - * Parameters defined by the L1 position control task + * Parameters defined by the fixed-wing attitude control task * * @author Lorenz Meier */ -- cgit v1.2.3 From cc8e85ce7e4e79a217edd40f64f6870b0ab72bb3 Mon Sep 17 00:00:00 2001 From: marco Date: Thu, 21 Nov 2013 22:24:16 +0100 Subject: mkblctrl scans now i2c3 and i2c1 bir connected esc's --- src/drivers/mkblctrl/mkblctrl.cpp | 80 +++++++++++++++++++++++++++++++++------ 1 file changed, 69 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 9442ae906..bd058e5d0 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -127,7 +127,7 @@ public: int set_overrideSecurityChecks(bool overrideSecurityChecks); int set_px4mode(int px4mode); int set_frametype(int frametype); - unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput); + unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: static const unsigned _max_actuators = MAX_MOTORS; @@ -726,8 +726,12 @@ MK::mk_servo_arm(bool status) unsigned int -MK::mk_check_for_blctrl(unsigned int count, bool showOutput) +MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) { + if(initI2C) { + I2C::init(); + } + _retries = 50; uint8_t foundMotorCount = 0; @@ -1366,7 +1370,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, /* count used motors */ do { - if (g_mk->mk_check_for_blctrl(8, false) != 0) { + if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { shouldStop = 4; } else { @@ -1376,7 +1380,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, sleep(1); } while (shouldStop < 3); - g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true)); + g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); /* (re)set the PWM output mode */ g_mk->set_mode(servo_mode); @@ -1414,6 +1418,52 @@ mk_start(unsigned bus, unsigned motors, char *device_path) } +int +mk_check_for_i2c_esc_bus(char *device_path, int motors) +{ + int ret; + + if (g_mk == nullptr) { + + g_mk = new MK(3, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 3; + } + + } + + + g_mk = new MK(1, device_path); + + if (g_mk == nullptr) { + return -1; + + } else { + ret = g_mk->mk_check_for_blctrl(8, false, true); + delete g_mk; + g_mk = nullptr; + + if (ret > 0) { + return 1; + } + + } + } + + return -1; +} + + + } // namespace extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); @@ -1424,7 +1474,7 @@ mkblctrl_main(int argc, char *argv[]) PortMode port_mode = PORT_FULL_PWM; int pwm_update_rate_in_hz = UPDATE_RATE; int motorcount = 8; - int bus = 1; + int bus = -1; int px4mode = MAPPING_PX4; int frametype = FRAME_PLUS; // + plus is default bool motortest = false; @@ -1517,15 +1567,23 @@ mkblctrl_main(int argc, char *argv[]) } - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); + if (bus != -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } - } else { - //////newMode = true; + if (bus != -1) { + if (g_mk == nullptr) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + + } else { + //////newMode = true; + } } - } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } /* parameter set ? */ if (newMode) { -- cgit v1.2.3 From d2e32f2fc56d723b566e8e935f86379cec4fee39 Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 22 Nov 2013 21:05:40 +0100 Subject: mkblctrl - hotfix for i2c scan --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index bd058e5d0..0a41bfef4 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1567,7 +1567,7 @@ mkblctrl_main(int argc, char *argv[]) } - if (bus != -1) { + if (bus == -1) { bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); } -- cgit v1.2.3 From ea9fcaa27ffc8e1edbb8c7a7dec768208944da3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 13:47:37 +0100 Subject: update the commander to only use local pos for landing detection when on rotary wing Conflicts: src/modules/commander/commander.cpp --- src/modules/commander/commander.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb78..dfd4d2f73 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); - if (status.condition_local_altitude_valid) { + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + // XXX: only respect the condition_landed when the local position is actually valid + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } @@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ - if (local_pos->z > -takeoff_alt || status->condition_landed) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } -- cgit v1.2.3 From 4e713a70835ee041f10d2f34745c9de81973c984 Mon Sep 17 00:00:00 2001 From: marco Date: Tue, 26 Nov 2013 19:01:43 +0100 Subject: motortest mode enhanced --- src/drivers/mkblctrl/mkblctrl.cpp | 79 +++++++++++++++++++++++---------------- 1 file changed, 46 insertions(+), 33 deletions(-) (limited to 'src') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 0a41bfef4..30d6069b3 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -91,7 +91,7 @@ #define MOTOR_STATE_PRESENT_MASK 0x80 #define MOTOR_STATE_ERROR_MASK 0x7F #define MOTOR_SPINUP_COUNTER 30 -#define ESC_UORB_PUBLISH_DELAY 200 +#define ESC_UORB_PUBLISH_DELAY 500000 class MK : public device::I2C { @@ -661,7 +661,7 @@ MK::task_main() * Only update esc topic every half second. */ - if (hrt_absolute_time() - esc.timestamp > 500000) { + if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) { esc.counter++; esc.timestamp = hrt_absolute_time(); esc.esc_count = (uint8_t) _num_outputs; @@ -955,6 +955,7 @@ MK::mk_servo_test(unsigned int chan) if (_motor >= _num_outputs) { _motor = -1; _motortest = false; + fprintf(stderr, "[mkblctrl] Motortest finished...\n"); } } @@ -1557,41 +1558,53 @@ mkblctrl_main(int argc, char *argv[]) if (showHelp) { fprintf(stderr, "mkblctrl: help:\n"); - fprintf(stderr, " [-mkmode frame{+/x}] [-b i2c_bus_number] [-d devicename] [-t] [--override-security-checks] [-h / --help]\n\n"); - fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); - fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n"); + fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); + fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); + fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t -t \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n"); fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, "\n"); + fprintf(stderr, "Motortest:\n"); + fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n"); + fprintf(stderr, "mkblctrl -t\n"); + fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n"); exit(1); } - if (bus == -1) { - bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); - } - - if (bus != -1) { - if (g_mk == nullptr) { - if (mk_start(bus, motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - - } else { - //////newMode = true; - } - } - } else { - errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); - - } - - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); - } - - /* test, etc. here g*/ - - exit(0); + if (!motortest) { + if (g_mk == nullptr) { + if (bus == -1) { + bus = mk_check_for_i2c_esc_bus(devicepath, motorcount); + } + + if (bus != -1) { + if (mk_start(bus, motorcount, devicepath) != OK) { + errx(1, "failed to start the MK-BLCtrl driver"); + } + } else { + errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)"); + } + + /* parameter set ? */ + if (newMode) { + /* switch parameter */ + return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks); + } + + exit(0); + } else { + errx(1, "MK-BLCtrl driver already running"); + } + + } else { + if (g_mk == nullptr) { + errx(1, "MK-BLCtrl driver not running. You have to start it first."); + + } else { + g_mk->set_motor_test(motortest); + exit(0); + + } + } } -- cgit v1.2.3 From 3c027a8e4d5e5e484a37f1026b6fa7835176426f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 27 Nov 2013 23:04:49 +0400 Subject: Various HIL-related fixes --- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++------- .../position_estimator_inav/position_estimator_inav_main.c | 11 +++++------ 2 files changed, 9 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bfccd5fb0..7b6fad658 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -382,16 +382,15 @@ handle_message(mavlink_message_t *msg) /* hil gyro */ static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_counter = hil_counter; hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = hil_counter; /* accelerometer */ - hil_sensors.accelerometer_counter = hil_counter; static const float mg2ms2 = 9.8f / 1000.0f; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -401,6 +400,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = hil_counter; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -409,7 +409,6 @@ handle_message(mavlink_message_t *msg) /* magnetometer */ float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_counter = hil_counter; hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; @@ -419,15 +418,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = hil_counter; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - - hil_sensors.gyro_counter = hil_counter; - hil_sensors.magnetometer_counter = hil_counter; - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.baro_counter = hil_counter; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 10007bf96..3084b6d92 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -236,13 +236,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - errx(1, "subscriptions poll error on init."); + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter > baro_counter) { + if (wait_baro && sensor.baro_counter != baro_counter) { baro_counter = sensor.baro_counter; /* mean calculation over several measurements */ @@ -320,8 +320,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (ret < 0) { /* poll error */ - warnx("subscriptions poll error."); - thread_should_exit = true; + mavlink_log_info(mavlink_fd, "[inav] poll error on init"); continue; } else if (ret > 0) { @@ -355,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accel_counter) { + if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -381,7 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_updates++; } - if (sensor.baro_counter > baro_counter) { + if (sensor.baro_counter != baro_counter) { baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; -- cgit v1.2.3 From bcd745fb0d32da04efd5ed34252e35dd2d3a3ffd Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Fri, 29 Nov 2013 02:05:15 +0900 Subject: SO(3) estimator and quaternion receive by mavlink implemented --- makefiles/config_px4fmu-v1_default.mk | 1 + src/modules/attitude_estimator_so3_comp/README | 4 +- .../attitude_estimator_so3_comp_main.cpp | 430 ++++++--------------- src/modules/mavlink/orb_listener.c | 15 +- 4 files changed, 136 insertions(+), 314 deletions(-) (limited to 'src') diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 306827086..a8aed6bb4 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -72,6 +72,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3_comp MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README index 79c50a531..02ab66ee4 100644 --- a/src/modules/attitude_estimator_so3_comp/README +++ b/src/modules/attitude_estimator_so3_comp/README @@ -1,5 +1,3 @@ Synopsis - nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200 - -Option -d is for debugging packet. See code for detailed packet structure. + nsh> attitude_estimator_so3_comp start \ No newline at end of file diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 86bda3c75..e12c0e16a 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -56,20 +56,16 @@ extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]) static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ + +//! Auxiliary variables to reduce number of repeated operations static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static bool bFilterInit = false; - -//! Auxiliary variables to reduce number of repeated operations static float q0q0, q0q1, q0q2, q0q3; static float q1q1, q1q2, q1q3; static float q2q2, q2q3; static float q3q3; - -//! Serial packet related -static int uart; -static int baudrate; +static bool bFilterInit = false; /** * Mainloop of attitude_estimator_so3_comp. @@ -81,15 +77,18 @@ int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +/* Function prototypes */ +float invSqrt(float number); +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d ] [-b ]\n" - "-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n" - "ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n"); + fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); exit(1); } @@ -106,12 +105,10 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) if (argc < 1) usage("missing command"); - - if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("attitude_estimator_so3_comp already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -120,20 +117,20 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12400, + 14000, attitude_estimator_so3_comp_thread_main, - (const char **)argv); + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { thread_should_exit = true; - while(thread_running){ + while (thread_running){ usleep(200000); - printf("."); } - printf("terminated."); + + warnx("stopped"); exit(0); } @@ -157,7 +154,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[]) //--------------------------------------------------------------------------------------------------- // Fast inverse square-root // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) { +float invSqrt(float number) +{ volatile long i; volatile float x, y; volatile const float f = 1.5F; @@ -221,48 +219,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa q3q3 = q3 * q3; } -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) { +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) +{ float recipNorm; float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - //! Make filter converge to initial solution faster - //! This function assumes you are in static position. - //! WARNING : in case air reboot, this can cause problem. But this is very - //! unlikely happen. - if(bFilterInit == false) - { + // Make filter converge to initial solution faster + // This function assumes you are in static position. + // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. + if(bFilterInit == false) { NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); bFilterInit = true; } //! If magnetometer measurement is available, use it. - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { float hx, hy, hz, bx, bz; float halfwx, halfwy, halfwz; // Normalise magnetometer measurement // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); + bx = sqrt(hx * hx + hy * hy); + bz = hz; - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); } // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) @@ -293,7 +290,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki gyro_bias[1] += twoKi * halfey * dt; gyro_bias[2] += twoKi * halfez * dt; - gx += gyro_bias[0]; // apply integral feedback + + // apply integral feedback + gx += gyro_bias[0]; gy += gyro_bias[1]; gz += gyro_bias[2]; } @@ -337,208 +336,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl q3 *= recipNorm; // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void send_uart_byte(char c) -{ - write(uart,&c,1); -} - -void send_uart_bytes(uint8_t *data, int length) -{ - write(uart,data,(size_t)(sizeof(uint8_t)*length)); -} - -void send_uart_float(float f) { - uint8_t * b = (uint8_t *) &f; - - //! Assume float is 4-bytes - for(int i=0; i<4; i++) { - - uint8_t b1 = (b[i] >> 4) & 0x0f; - uint8_t b2 = (b[i] & 0x0f); - - uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10; - uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10; - - send_uart_bytes(&c1,1); - send_uart_bytes(&c2,1); - } -} - -void send_uart_float_arr(float *arr, int length) -{ - for(int i=0;i, default : /dev/ttyS2 - //! -b , default : 115200 - while ((ch = getopt(argc,argv,"d:b:")) != EOF){ - switch(ch){ - case 'b': - baudrate = strtoul(optarg, NULL, 10); - if(baudrate == 0) - printf("invalid baud rate '%s'",optarg); - break; - case 'd': - device_name = optarg; - debug_mode = true; - break; - default: - usage("invalid argument"); - } - } - - if(debug_mode){ - printf("Opening debugging port for 3D visualization\n"); - uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) - printf("could not open %s", device_name); - else - printf("Open port success\n"); - } - - // print text - printf("Nonlinear SO3 Attitude Estimator initialized..\n\n"); - fflush(stdout); - - int overloadcounter = 19; - - /* store start time to guard against too slow update rates */ - uint64_t last_run = hrt_absolute_time(); + warnx("main thread started"); struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); @@ -555,8 +389,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 4); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -565,17 +399,15 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ - orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t att_pub = -1; + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; int printcounter = 0; thread_running = true; - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs @@ -588,9 +420,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* initialize parameter handles */ parameters_init(&so3_comp_param_handles); + parameters_update(&so3_comp_param_handles, &so3_comp_params); uint64_t start_time = hrt_absolute_time(); bool initialized = false; + bool state_initialized = false; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; @@ -615,12 +449,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - fprintf(stderr, - "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); + warnx("WARNING: Not getting sensors - sensor app running?"); } - } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -644,11 +475,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds gyro_offsets[2] += raw.gyro_rad_s[2]; offset_count++; - if (hrt_absolute_time() - start_time > 3000000LL) { + if (hrt_absolute_time() > start_time + 3000000l) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; + warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); } } else { @@ -668,9 +500,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[0] = raw.timestamp; } - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { @@ -696,31 +528,14 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds mag[1] = raw.magnetometer_ga[1]; mag[2] = raw.magnetometer_ga[2]; - uint64_t now = hrt_absolute_time(); - unsigned int time_elapsed = now - last_run; - last_run = now; - - if (time_elapsed > loop_interval_alarm) { - //TODO: add warning, cpu overload here - // if (overloadcounter == 20) { - // printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); - // overloadcounter = 0; - // } - - overloadcounter++; - } - - static bool const_initialized = false; - /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized && dt < 0.05f && dt > 0.005f) { - dt = 0.005f; - parameters_update(&so3_comp_param_handles, &so3_comp_params); - const_initialized = true; + if (!state_initialized && dt < 0.05f && dt > 0.001f) { + state_initialized = true; + warnx("state initialized"); } /* do not execute the filter if not initialized */ - if (!const_initialized) { + if (!state_initialized) { continue; } @@ -728,18 +543,23 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); + NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], + -acc[0], -acc[1], -acc[2], + mag[0], mag[1], mag[2], + so3_comp_params.Kp, + so3_comp_params.Ki, + dt); // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 + Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 + Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 + Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 + Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 + Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 + Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 + Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 + Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 //1-2-3 Representation. //Equation (290) @@ -747,29 +567,42 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]); //! Yaw + euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - /* Do something */ + // Publish only finite euler angles + att.roll = euler[0] - so3_comp_params.roll_off; + att.pitch = euler[1] - so3_comp_params.pitch_off; + att.yaw = euler[2] - so3_comp_params.yaw_off; } else { /* due to inputs or numerical failure the output is invalid, skip it */ + // Due to inputs or numerical failure the output is invalid + warnx("infinite euler angles, rotation matrix:"); + warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); + warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); + warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + // Don't publish anything continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data); + if (last_data > 0 && raw.timestamp > last_data + 12000) { + warnx("sensor data missed"); + } last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; + + // Quaternion + att.q[0] = q0; + att.q[1] = q1; + att.q[2] = q2; + att.q[3] = q3; + att.q_valid = true; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - - //! Euler angle rate. But it needs to be investigated again. + // Euler angle rate. But it needs to be investigated again. /* att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); @@ -783,53 +616,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = 0; att.yawacc = 0; - //! Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - /* TODO: Bias estimation required */ memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; - - if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { - // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); - + + // Publish + if (att_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } else { warnx("NaN in roll/pitch/yaw estimate!"); + orb_advertise(ORB_ID(vehicle_attitude), &att); } perf_end(so3_comp_loop_perf); - - //! This will print out debug packet to visualization software - if(debug_mode) - { - float quat[4]; - quat[0] = q0; - quat[1] = q1; - quat[2] = q2; - quat[3] = q3; - send_uart_float_arr(quat,4); - send_uart_byte('\n'); - } } } } loopcounter++; - }// while + } thread_running = false; - /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); - return 0; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index abc91d34f..564bf806a 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -242,7 +242,7 @@ l_vehicle_attitude(const struct listener *l) att.rollspeed, att.pitchspeed, att.yawspeed); - + /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { @@ -252,6 +252,19 @@ l_vehicle_attitude(const struct listener *l) float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } + + /* send quaternion values if it exists */ + if(att.q_valid) { + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); + } } attitude_counter++; -- cgit v1.2.3 From b3f1adc54bac36b8da16651a545835bb1877f883 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Fri, 29 Nov 2013 02:35:49 +0900 Subject: SO3 estimator code has been cleaned --- makefiles/config_px4fmu-v1_backside.mk | 3 +- makefiles/config_px4fmu-v1_default.mk | 4 +- makefiles/config_px4fmu-v2_default.mk | 3 +- src/modules/attitude_estimator_so3/README | 3 + .../attitude_estimator_so3_main.cpp | 678 +++++++++++++++++++++ .../attitude_estimator_so3_params.c | 86 +++ .../attitude_estimator_so3_params.h | 67 ++ src/modules/attitude_estimator_so3/module.mk | 8 + src/modules/attitude_estimator_so3_comp/README | 3 - .../attitude_estimator_so3_comp_main.cpp | 645 -------------------- .../attitude_estimator_so3_comp_params.c | 63 -- .../attitude_estimator_so3_comp_params.h | 44 -- src/modules/attitude_estimator_so3_comp/module.mk | 8 - 13 files changed, 848 insertions(+), 767 deletions(-) create mode 100644 src/modules/attitude_estimator_so3/README create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c create mode 100755 src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h create mode 100644 src/modules/attitude_estimator_so3/module.mk delete mode 100644 src/modules/attitude_estimator_so3_comp/README delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c delete mode 100755 src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h delete mode 100644 src/modules/attitude_estimator_so3_comp/module.mk (limited to 'src') diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index e1a42530b..5ecf93a3a 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -69,12 +69,13 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # #MODULES += modules/attitude_estimator_ekf MODULES += modules/att_pos_estimator_ekf #MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator +MODULES += modules/attitude_estimator_so3 # # Vehicle Control diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a8aed6bb4..25c22f1fd 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -69,10 +69,10 @@ MODULES += modules/mavlink_onboard MODULES += modules/gpio_led # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3_comp +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..015a7387a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -68,9 +68,10 @@ MODULES += modules/mavlink MODULES += modules/mavlink_onboard # -# Estimation modules (EKF / other filters) +# Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/attitude_estimator_so3/README b/src/modules/attitude_estimator_so3/README new file mode 100644 index 000000000..02ab66ee4 --- /dev/null +++ b/src/modules/attitude_estimator_so3/README @@ -0,0 +1,3 @@ +Synopsis + + nsh> attitude_estimator_so3_comp start \ No newline at end of file diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp new file mode 100755 index 000000000..e79726613 --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -0,0 +1,678 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_main.cpp + * + * Implementation of nonlinear complementary filters on the SO(3). + * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. + * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. + * + * Theory of nonlinear complementary filters on the SO(3) is based on [1]. + * Quaternion realization of [1] is based on [2]. + * Optmized quaternion update code is based on Sebastian Madgwick's implementation. + * + * References + * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 + * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif +#include "attitude_estimator_so3_params.h" +#ifdef __cplusplus +} +#endif + +extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]); + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int attitude_estimator_so3_task; /**< Handle of deamon task / thread */ + +//! Auxiliary variables to reduce number of repeated operations +static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ +static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ +static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ +static float q0q0, q0q1, q0q2, q0q3; +static float q1q1, q1q2, q1q3; +static float q2q2, q2q3; +static float q3q3; +static bool bFilterInit = false; + +/** + * Mainloop of attitude_estimator_so3. + */ +int attitude_estimator_so3_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +/* Function prototypes */ +float invSqrt(float number); +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n"); + exit(1); +} + +/** + * The attitude_estimator_so3 app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int attitude_estimator_so3_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 14000, + attitude_estimator_so3_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + + while (thread_running){ + usleep(200000); + } + + warnx("stopped"); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("running"); + exit(0); + + } else { + warnx("not started"); + exit(1); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +//--------------------------------------------------------------------------------------------------- +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root +float invSqrt(float number) +{ + volatile long i; + volatile float x, y; + volatile const float f = 1.5F; + + x = number * 0.5F; + y = number; + i = * (( long * ) &y); + i = 0x5f375a86 - ( i >> 1 ); + y = * (( float * ) &i); + y = y * ( f - ( x * y * y ) ); + return y; +} + +//! Using accelerometer, sense the gravity vector. +//! Using magnetometer, sense yaw. +void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + // auxillary variables to reduce number of repeated operations, for 1st pass + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; +} + +void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) +{ + float recipNorm; + float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; + + // Make filter converge to initial solution faster + // This function assumes you are in static position. + // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. + if(bFilterInit == false) { + NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); + bFilterInit = true; + } + + //! If magnetometer measurement is available, use it. + if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { + float hx, hy, hz, bx, bz; + float halfwx, halfwy, halfwz; + + // Normalise magnetometer measurement + // Will sqrt work better? PX4 system is powerful enough? + recipNorm = invSqrt(mx * mx + my * my + mz * mz); + mx *= recipNorm; + my *= recipNorm; + mz *= recipNorm; + + // Reference direction of Earth's magnetic field + hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); + hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); + hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); + bx = sqrt(hx * hx + hy * hy); + bz = hz; + + // Estimated direction of magnetic field + halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); + halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); + halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += (my * halfwz - mz * halfwy); + halfey += (mz * halfwx - mx * halfwz); + halfez += (mx * halfwy - my * halfwx); + } + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + float halfvx, halfvy, halfvz; + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and magnetic field + halfvx = q1q3 - q0q2; + halfvy = q0q1 + q2q3; + halfvz = q0q0 - 0.5f + q3q3; + + // Error is sum of cross product between estimated direction and measured direction of field vectors + halfex += ay * halfvz - az * halfvy; + halfey += az * halfvx - ax * halfvz; + halfez += ax * halfvy - ay * halfvx; + } + + // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer + if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { + // Compute and apply integral feedback if enabled + if(twoKi > 0.0f) { + gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki + gyro_bias[1] += twoKi * halfey * dt; + gyro_bias[2] += twoKi * halfez * dt; + + // apply integral feedback + gx += gyro_bias[0]; + gy += gyro_bias[1]; + gz += gyro_bias[2]; + } + else { + gyro_bias[0] = 0.0f; // prevent integral windup + gyro_bias[1] = 0.0f; + gyro_bias[2] = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + //! Integrate rate of change of quaternion +#if 0 + gx *= (0.5f * dt); // pre-multiply common factors + gy *= (0.5f * dt); + gz *= (0.5f * dt); +#endif + + // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. + //! q_k = q_{k-1} + dt*\dot{q} + //! \dot{q} = 0.5*q \otimes P(\omega) + dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); + dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); + dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); + dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); + + q0 += dt*dq0; + q1 += dt*dq1; + q2 += dt*dq2; + q3 += dt*dq3; + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + // Auxiliary variables to avoid repeated arithmetic + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; +} + +/* + * Nonliner complementary filter on SO(3), attitude estimator main function. + * + * Estimates the attitude once started. + * + * @param argc number of commandline arguments (plus command name) + * @param argv strings containing the arguments + */ +int attitude_estimator_so3_thread_main(int argc, char *argv[]) +{ + const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + //! Time constant + float dt = 0.005f; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + /* Initialization */ + float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ + float acc[3] = {0.0f, 0.0f, 0.0f}; + float gyro[3] = {0.0f, 0.0f, 0.0f}; + float mag[3] = {0.0f, 0.0f, 0.0f}; + + warnx("main thread started"); + + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + + //! Initialize attitude vehicle uORB message. + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + + uint64_t last_data = 0; + uint64_t last_measurement = 0; + + /* subscribe to raw data */ + int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ + orb_set_interval(sub_raw, 3); + + /* subscribe to param changes */ + int sub_params = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + + /* advertise attitude */ + //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + //orb_advert_t att_pub = -1; + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + + int loopcounter = 0; + int printcounter = 0; + + thread_running = true; + + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; + // XXX write this out to perf regs + + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + + struct attitude_estimator_so3_params so3_comp_params; + struct attitude_estimator_so3_param_handles so3_comp_param_handles; + + /* initialize parameter handles */ + parameters_init(&so3_comp_param_handles); + parameters_update(&so3_comp_param_handles, &so3_comp_params); + + uint64_t start_time = hrt_absolute_time(); + bool initialized = false; + bool state_initialized = false; + + float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; + unsigned offset_count = 0; + + /* register the perf counter */ + perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3"); + + /* Main loop*/ + while (!thread_should_exit) { + + struct pollfd fds[2]; + fds[0].fd = sub_raw; + fds[0].events = POLLIN; + fds[1].fd = sub_params; + fds[1].events = POLLIN; + int ret = poll(fds, 2, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* check if we're in HIL - not getting sensor data is fine then */ + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); + + if (!control_mode.flag_system_hil_enabled) { + warnx("WARNING: Not getting sensors - sensor app running?"); + } + } else { + /* only update parameters if they changed */ + if (fds[1].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), sub_params, &update); + + /* update parameters */ + parameters_update(&so3_comp_param_handles, &so3_comp_params); + } + + /* only run filter if sensor values changed */ + if (fds[0].revents & POLLIN) { + + /* get latest measurements */ + orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + + if (!initialized) { + + gyro_offsets[0] += raw.gyro_rad_s[0]; + gyro_offsets[1] += raw.gyro_rad_s[1]; + gyro_offsets[2] += raw.gyro_rad_s[2]; + offset_count++; + + if (hrt_absolute_time() > start_time + 3000000l) { + initialized = true; + gyro_offsets[0] /= offset_count; + gyro_offsets[1] /= offset_count; + gyro_offsets[2] /= offset_count; + warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + } + + } else { + + perf_begin(so3_comp_loop_perf); + + /* Calculate data time difference in seconds */ + dt = (raw.timestamp - last_measurement) / 1000000.0f; + last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; + + /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + + gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; + + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } + + acc[0] = raw.accelerometer_m_s2[0]; + acc[1] = raw.accelerometer_m_s2[1]; + acc[2] = raw.accelerometer_m_s2[2]; + + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } + + mag[0] = raw.magnetometer_ga[0]; + mag[1] = raw.magnetometer_ga[1]; + mag[2] = raw.magnetometer_ga[2]; + + /* initialize with good values once we have a reasonable dt estimate */ + if (!state_initialized && dt < 0.05f && dt > 0.001f) { + state_initialized = true; + warnx("state initialized"); + } + + /* do not execute the filter if not initialized */ + if (!state_initialized) { + continue; + } + + uint64_t timing_start = hrt_absolute_time(); + + // NOTE : Accelerometer is reversed. + // Because proper mount of PX4 will give you a reversed accelerometer readings. + NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], + -acc[0], -acc[1], -acc[2], + mag[0], mag[1], mag[2], + so3_comp_params.Kp, + so3_comp_params.Ki, + dt); + + // Convert q->R, This R converts inertial frame to body frame. + Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 + Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 + Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 + Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 + Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 + Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 + Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 + Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 + Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 + + //1-2-3 Representation. + //Equation (290) + //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel. + // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. + euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll + euler[1] = -asinf(Rot_matrix[2]); //! Pitch + euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw + + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + // Publish only finite euler angles + att.roll = euler[0] - so3_comp_params.roll_off; + att.pitch = euler[1] - so3_comp_params.pitch_off; + att.yaw = euler[2] - so3_comp_params.yaw_off; + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + // Due to inputs or numerical failure the output is invalid + warnx("infinite euler angles, rotation matrix:"); + warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); + warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); + warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + // Don't publish anything + continue; + } + + if (last_data > 0 && raw.timestamp > last_data + 12000) { + warnx("sensor data missed"); + } + + last_data = raw.timestamp; + + /* send out */ + att.timestamp = raw.timestamp; + + // Quaternion + att.q[0] = q0; + att.q[1] = q1; + att.q[2] = q2; + att.q[3] = q3; + att.q_valid = true; + + // Euler angle rate. But it needs to be investigated again. + /* + att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); + att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); + att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); + */ + att.rollspeed = gyro[0]; + att.pitchspeed = gyro[1]; + att.yawspeed = gyro[2]; + + att.rollacc = 0; + att.pitchacc = 0; + att.yawacc = 0; + + /* TODO: Bias estimation required */ + memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(float)*9); + att.R_valid = true; + + // Publish + if (att_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); + } else { + warnx("NaN in roll/pitch/yaw estimate!"); + orb_advertise(ORB_ID(vehicle_attitude), &att); + } + + perf_end(so3_comp_loop_perf); + } + } + } + + loopcounter++; + } + + thread_running = false; + + return 0; +} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c new file mode 100755 index 000000000..0c8d522b4 --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_params.c + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include "attitude_estimator_so3_params.h" + +/* This is filter gain for nonlinear SO3 complementary filter */ +/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. + Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. + If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which + will compensate gyro bias which depends on temperature and vibration of your vehicle */ +PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. + //! You can set this gain higher if you want more fast response. + //! But note that higher gain will give you also higher overshoot. +PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) + //! This gain is depend on your vehicle status. + +/* offsets in roll, pitch and yaw of sensor plane and body */ +PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f); + +int parameters_init(struct attitude_estimator_so3_param_handles *h) +{ + /* Filter gain parameters */ + h->Kp = param_find("SO3_COMP_KP"); + h->Ki = param_find("SO3_COMP_KI"); + + /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ + h->roll_off = param_find("SO3_ROLL_OFFS"); + h->pitch_off = param_find("SO3_PITCH_OFFS"); + h->yaw_off = param_find("SO3_YAW_OFFS"); + + return OK; +} + +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p) +{ + /* Update filter gain */ + param_get(h->Kp, &(p->Kp)); + param_get(h->Ki, &(p->Ki)); + + /* Update attitude offset */ + param_get(h->roll_off, &(p->roll_off)); + param_get(h->pitch_off, &(p->pitch_off)); + param_get(h->yaw_off, &(p->yaw_off)); + + return OK; +} diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h new file mode 100755 index 000000000..dfb4cad05 --- /dev/null +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Hyon Lim + * Anton Babushkin + * + * 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 attitude_estimator_so3_params.h + * + * Parameters for nonlinear complementary filters on the SO(3). + */ + +#include + +struct attitude_estimator_so3_params { + float Kp; + float Ki; + float roll_off; + float pitch_off; + float yaw_off; +}; + +struct attitude_estimator_so3_param_handles { + param_t Kp, Ki; + param_t roll_off, pitch_off, yaw_off; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct attitude_estimator_so3_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p); diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk new file mode 100644 index 000000000..e29bb16a6 --- /dev/null +++ b/src/modules/attitude_estimator_so3/module.mk @@ -0,0 +1,8 @@ +# +# Attitude estimator (Nonlinear SO(3) complementary Filter) +# + +MODULE_COMMAND = attitude_estimator_so3 + +SRCS = attitude_estimator_so3_main.cpp \ + attitude_estimator_so3_params.c diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README deleted file mode 100644 index 02ab66ee4..000000000 --- a/src/modules/attitude_estimator_so3_comp/README +++ /dev/null @@ -1,3 +0,0 @@ -Synopsis - - nsh> attitude_estimator_so3_comp start \ No newline at end of file diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp deleted file mode 100755 index e12c0e16a..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ /dev/null @@ -1,645 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_main.c - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif -#include "attitude_estimator_so3_comp_params.h" -#ifdef __cplusplus -} -#endif - -extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]); - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int attitude_estimator_so3_comp_task; /**< Handle of deamon task / thread */ - -//! Auxiliary variables to reduce number of repeated operations -static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ -static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f; /** quaternion of sensor frame relative to auxiliary frame */ -static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */ -static float q0q0, q0q1, q0q2, q0q3; -static float q1q1, q1q2, q1q3; -static float q2q2, q2q3; -static float q3q3; -static bool bFilterInit = false; - -/** - * Mainloop of attitude_estimator_so3_comp. - */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/* Function prototypes */ -float invSqrt(float number); -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz); -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status}\n"); - exit(1); -} - -/** - * The attitude_estimator_so3_comp app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int attitude_estimator_so3_comp_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 14000, - attitude_estimator_so3_comp_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - - while (thread_running){ - usleep(200000); - } - - warnx("stopped"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("running"); - exit(0); - - } else { - warnx("not started"); - exit(1); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -//--------------------------------------------------------------------------------------------------- -// Fast inverse square-root -// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root -float invSqrt(float number) -{ - volatile long i; - volatile float x, y; - volatile const float f = 1.5F; - - x = number * 0.5F; - y = number; - i = * (( long * ) &y); - i = 0x5f375a86 - ( i >> 1 ); - y = * (( float * ) &i); - y = y * ( f - ( x * y * y ) ); - return y; -} - -//! Using accelerometer, sense the gravity vector. -//! Using magnetometer, sense yaw. -void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - // auxillary variables to reduce number of repeated operations, for 1st pass - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) -{ - float recipNorm; - float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f; - - // Make filter converge to initial solution faster - // This function assumes you are in static position. - // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen. - if(bFilterInit == false) { - NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz); - bFilterInit = true; - } - - //! If magnetometer measurement is available, use it. - if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { - float hx, hy, hz, bx, bz; - float halfwx, halfwy, halfwz; - - // Normalise magnetometer measurement - // Will sqrt work better? PX4 system is powerful enough? - recipNorm = invSqrt(mx * mx + my * my + mz * mz); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2); - bx = sqrt(hx * hx + hy * hy); - bz = hz; - - // Estimated direction of magnetic field - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += (my * halfwz - mz * halfwy); - halfey += (mz * halfwx - mx * halfwz); - halfez += (mx * halfwy - my * halfwx); - } - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - float halfvx, halfvy, halfvz; - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and magnetic field - halfvx = q1q3 - q0q2; - halfvy = q0q1 + q2q3; - halfvz = q0q0 - 0.5f + q3q3; - - // Error is sum of cross product between estimated direction and measured direction of field vectors - halfex += ay * halfvz - az * halfvy; - halfey += az * halfvx - ax * halfvz; - halfez += ax * halfvy - ay * halfvx; - } - - // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer - if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) { - // Compute and apply integral feedback if enabled - if(twoKi > 0.0f) { - gyro_bias[0] += twoKi * halfex * dt; // integral error scaled by Ki - gyro_bias[1] += twoKi * halfey * dt; - gyro_bias[2] += twoKi * halfez * dt; - - // apply integral feedback - gx += gyro_bias[0]; - gy += gyro_bias[1]; - gz += gyro_bias[2]; - } - else { - gyro_bias[0] = 0.0f; // prevent integral windup - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - //! Integrate rate of change of quaternion -#if 0 - gx *= (0.5f * dt); // pre-multiply common factors - gy *= (0.5f * dt); - gz *= (0.5f * dt); -#endif - - // Time derivative of quaternion. q_dot = 0.5*q\otimes omega. - //! q_k = q_{k-1} + dt*\dot{q} - //! \dot{q} = 0.5*q \otimes P(\omega) - dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz); - dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy); - dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx); - dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx); - - q0 += dt*dq0; - q1 += dt*dq1; - q2 += dt*dq2; - q3 += dt*dq3; - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; -} - -/* - * Nonliner complementary filter on SO(3), attitude estimator main function. - * - * Estimates the attitude once started. - * - * @param argc number of commandline arguments (plus command name) - * @param argv strings containing the arguments - */ -int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]) -{ - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - - //! Time constant - float dt = 0.005f; - - /* output euler angles */ - float euler[3] = {0.0f, 0.0f, 0.0f}; - - /* Initialization */ - float Rot_matrix[9] = {1.f, 0.0f, 0.0f, 0.0f, 1.f, 0.0f, 0.0f, 0.0f, 1.f }; /**< init: identity matrix */ - float acc[3] = {0.0f, 0.0f, 0.0f}; - float gyro[3] = {0.0f, 0.0f, 0.0f}; - float mag[3] = {0.0f, 0.0f, 0.0f}; - - warnx("main thread started"); - - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - - //! Initialize attitude vehicle uORB message. - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - - uint64_t last_data = 0; - uint64_t last_measurement = 0; - - /* subscribe to raw data */ - int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); - /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ - orb_set_interval(sub_raw, 3); - - /* subscribe to param changes */ - int sub_params = orb_subscribe(ORB_ID(parameter_update)); - - /* subscribe to control mode */ - int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - - /* advertise attitude */ - //orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); - //orb_advert_t att_pub = -1; - orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - int loopcounter = 0; - int printcounter = 0; - - thread_running = true; - - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; - // XXX write this out to perf regs - - /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; - uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - - struct attitude_estimator_so3_comp_params so3_comp_params; - struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles; - - /* initialize parameter handles */ - parameters_init(&so3_comp_param_handles); - parameters_update(&so3_comp_param_handles, &so3_comp_params); - - uint64_t start_time = hrt_absolute_time(); - bool initialized = false; - bool state_initialized = false; - - float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; - unsigned offset_count = 0; - - /* register the perf counter */ - perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp"); - - /* Main loop*/ - while (!thread_should_exit) { - - struct pollfd fds[2]; - fds[0].fd = sub_raw; - fds[0].events = POLLIN; - fds[1].fd = sub_params; - fds[1].events = POLLIN; - int ret = poll(fds, 2, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - - if (!control_mode.flag_system_hil_enabled) { - warnx("WARNING: Not getting sensors - sensor app running?"); - } - } else { - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), sub_params, &update); - - /* update parameters */ - parameters_update(&so3_comp_param_handles, &so3_comp_params); - } - - /* only run filter if sensor values changed */ - if (fds[0].revents & POLLIN) { - - /* get latest measurements */ - orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); - - if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() > start_time + 3000000l) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); - } - - } else { - - perf_begin(so3_comp_loop_perf); - - /* Calculate data time difference in seconds */ - dt = (raw.timestamp - last_measurement) / 1000000.0f; - last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; - - /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { - update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); - sensor_last_timestamp[0] = raw.timestamp; - } - - gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; - gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; - gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; - - /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { - update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; - } - - acc[0] = raw.accelerometer_m_s2[0]; - acc[1] = raw.accelerometer_m_s2[1]; - acc[2] = raw.accelerometer_m_s2[2]; - - /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { - update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; - } - - mag[0] = raw.magnetometer_ga[0]; - mag[1] = raw.magnetometer_ga[1]; - mag[2] = raw.magnetometer_ga[2]; - - /* initialize with good values once we have a reasonable dt estimate */ - if (!state_initialized && dt < 0.05f && dt > 0.001f) { - state_initialized = true; - warnx("state initialized"); - } - - /* do not execute the filter if not initialized */ - if (!state_initialized) { - continue; - } - - uint64_t timing_start = hrt_absolute_time(); - - // NOTE : Accelerometer is reversed. - // Because proper mount of PX4 will give you a reversed accelerometer readings. - NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], - -acc[0], -acc[1], -acc[2], - mag[0], mag[1], mag[2], - so3_comp_params.Kp, - so3_comp_params.Ki, - dt); - - // Convert q->R, This R converts inertial frame to body frame. - Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 - Rot_matrix[1] = 2.f * (q1*q2 + q0*q3); // 12 - Rot_matrix[2] = 2.f * (q1*q3 - q0*q2); // 13 - Rot_matrix[3] = 2.f * (q1*q2 - q0*q3); // 21 - Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22 - Rot_matrix[5] = 2.f * (q2*q3 + q0*q1); // 23 - Rot_matrix[6] = 2.f * (q1*q3 + q0*q2); // 31 - Rot_matrix[7] = 2.f * (q2*q3 - q0*q1); // 32 - Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33 - - //1-2-3 Representation. - //Equation (290) - //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel. - // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. - euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]); //! Roll - euler[1] = -asinf(Rot_matrix[2]); //! Pitch - euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]); //! Yaw - - /* swap values for next iteration, check for fatal inputs */ - if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { - // Publish only finite euler angles - att.roll = euler[0] - so3_comp_params.roll_off; - att.pitch = euler[1] - so3_comp_params.pitch_off; - att.yaw = euler[2] - so3_comp_params.yaw_off; - } else { - /* due to inputs or numerical failure the output is invalid, skip it */ - // Due to inputs or numerical failure the output is invalid - warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); - // Don't publish anything - continue; - } - - if (last_data > 0 && raw.timestamp > last_data + 12000) { - warnx("sensor data missed"); - } - - last_data = raw.timestamp; - - /* send out */ - att.timestamp = raw.timestamp; - - // Quaternion - att.q[0] = q0; - att.q[1] = q1; - att.q[2] = q2; - att.q[3] = q3; - att.q_valid = true; - - // Euler angle rate. But it needs to be investigated again. - /* - att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3); - att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3); - att.yawspeed = 2.0f*(-q3*dq0 -q2*dq1 + q1*dq2 + q0*dq3); - */ - att.rollspeed = gyro[0]; - att.pitchspeed = gyro[1]; - att.yawspeed = gyro[2]; - - att.rollacc = 0; - att.pitchacc = 0; - att.yawacc = 0; - - /* TODO: Bias estimation required */ - memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); - - /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(float)*9); - att.R_valid = true; - - // Publish - if (att_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); - } else { - warnx("NaN in roll/pitch/yaw estimate!"); - orb_advertise(ORB_ID(vehicle_attitude), &att); - } - - perf_end(so3_comp_loop_perf); - } - } - } - - loopcounter++; - } - - thread_running = false; - - return 0; -} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c deleted file mode 100755 index f962515df..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.c - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include "attitude_estimator_so3_comp_params.h" - -/* This is filter gain for nonlinear SO3 complementary filter */ -/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place. - Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP. - If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which - will compensate gyro bias which depends on temperature and vibration of your vehicle */ -PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time. - //! You can set this gain higher if you want more fast response. - //! But note that higher gain will give you also higher overshoot. -PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change) - //! This gain is depend on your vehicle status. - -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); - -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h) -{ - /* Filter gain parameters */ - h->Kp = param_find("SO3_COMP_KP"); - h->Ki = param_find("SO3_COMP_KI"); - - /* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */ - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); - - return OK; -} - -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p) -{ - /* Update filter gain */ - param_get(h->Kp, &(p->Kp)); - param_get(h->Ki, &(p->Ki)); - - /* Update attitude offset */ - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - - return OK; -} diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h deleted file mode 100755 index f00695630..000000000 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Author: Hyon Lim - * - * @file attitude_estimator_so3_comp_params.h - * - * Implementation of nonlinear complementary filters on the SO(3). - * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. - * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. - * - * Theory of nonlinear complementary filters on the SO(3) is based on [1]. - * Quaternion realization of [1] is based on [2]. - * Optmized quaternion update code is based on Sebastian Madgwick's implementation. - * - * References - * [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008 - * [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008 - */ - -#include - -struct attitude_estimator_so3_comp_params { - float Kp; - float Ki; - float roll_off; - float pitch_off; - float yaw_off; -}; - -struct attitude_estimator_so3_comp_param_handles { - param_t Kp, Ki; - param_t roll_off, pitch_off, yaw_off; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct attitude_estimator_so3_comp_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p); diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk deleted file mode 100644 index 92f43d920..000000000 --- a/src/modules/attitude_estimator_so3_comp/module.mk +++ /dev/null @@ -1,8 +0,0 @@ -# -# Attitude estimator (Nonlinear SO3 complementary Filter) -# - -MODULE_COMMAND = attitude_estimator_so3_comp - -SRCS = attitude_estimator_so3_comp_main.cpp \ - attitude_estimator_so3_comp_params.c -- cgit v1.2.3 From 7becbcdbd59c0842ff44e682df0f13fd067def10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:34:25 +0100 Subject: Made all usual suspects default to their custom names and only register the default name if its not already taken by someone else --- src/drivers/hmc5883/hmc5883.cpp | 7 ++++--- src/drivers/l3gd20/l3gd20.cpp | 7 +++++++ src/drivers/lsm303d/lsm303d.cpp | 33 ++++++++++++++++++++++++--------- src/drivers/mpu6000/mpu6000.cpp | 33 +++++++++++++++++++++++++-------- 4 files changed, 60 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff8..22ad301ff 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1256,7 +1257,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,7 +1289,7 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac5..d816b1a59 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -360,6 +360,13 @@ L3GD20::init() if (_reports == nullptr) goto out; + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 47109b67d..229b052a9 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -78,7 +78,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -551,6 +552,20 @@ LSM303D::init() reset(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); + + if (mag_ret == OK) { + log("default mag device"); + } + /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); @@ -1491,7 +1506,7 @@ LSM303D::print_registers() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), + CDev("LSM303D_mag", "/dev/lsm303d_mag"), _parent(parent) { } @@ -1556,7 +1571,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1567,7 +1582,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1575,7 +1590,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1610,10 +1625,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1639,10 +1654,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6bfa583fb..ce0010752 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -359,7 +362,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -468,6 +471,20 @@ MPU6000::init() /* fetch an initial set of measurements for advertisement */ measure(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + if (gyro_ret != OK) { _gyro_topic = -1; } else { @@ -1290,7 +1307,7 @@ MPU6000::print_info() } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent) { } @@ -1352,7 +1369,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1384,17 +1401,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1452,7 +1469,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From c72162cc5ae12e2e64f9a4fb86e205bab51a5b6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:44:29 +0100 Subject: Add also default descriptor for alternate sensors --- src/drivers/lsm303d/lsm303d.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 229b052a9..c9e4fe1f8 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -552,11 +552,21 @@ LSM303D::init() reset(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ + /* register the first instance as plain name, the 2nd as two and counting */ ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default accel device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* try to claim the generic accel node as well - it's OK if we fail at this */ @@ -564,6 +574,16 @@ LSM303D::init() if (mag_ret == OK) { log("default mag device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* advertise mag topic */ -- cgit v1.2.3 From 3f0f34a4c786e7b8baccf57b2c22eddd6ee7c97f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:55:10 +1100 Subject: ms5611: give cleaner SPI traces this makes logic traces cleaner by zeroing extra bytes written --- src/drivers/ms5611/ms5611_spi.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913b..85504b0bd 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -167,10 +167,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -241,18 +240,21 @@ MS5611_SPI::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); -- cgit v1.2.3 From a52e70ca93a78edeb8d14ae95f881e0415e13760 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:54:58 +1100 Subject: ms5611: removed unused variable --- src/drivers/ms5611/ms5611_spi.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 85504b0bd..65c23ae57 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { -- cgit v1.2.3 From b0bb5a34508c72efbbfc2ec622a2cd8a95e9df1d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:53:25 +1100 Subject: ms5611: change bus speed to 5MHz this gives 5MHz SPI bus speed (by asking for 6MHz due to timer granularity). Tests with a logic analyser show that the ms5611 is actually more reliable at 5MHz than lower speeds --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 65c23ae57..e9dff5a8b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), _prom(prom_buf) { } -- cgit v1.2.3 From 476070510eca3c1eb8c485b5f2d04061dfb24f88 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 22:54:02 +1100 Subject: lsm303d/l3gd20: change filters to 50Hz analog on-chip filters after discussion with Leonard these analog on-chip filters should be at 50Hz --- src/drivers/l3gd20/l3gd20.cpp | 11 ++++++++--- src/drivers/lsm303d/lsm303d.cpp | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac5..17b53bfa9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -93,10 +93,15 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 @@ -662,15 +667,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_70HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_100HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_100HZ; + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 47109b67d..356569f99 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -599,7 +599,12 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -- cgit v1.2.3 From 86ec1c37fa29ba4ffc1d54a0c438de1cd536f51c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Dec 2013 09:06:53 +1100 Subject: l3gd20: added retries to disable_i2c() --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 17b53bfa9..176ef648b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -715,8 +715,16 @@ L3GD20::stop() void L3GD20::disable_i2c(void) { - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); } void -- cgit v1.2.3 From 53f2dc8296d550df8933663465cf163ae523084a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:00 +1100 Subject: drv_hrt: added hrt_call_init() and hrt_call_delay() APIs hrt_call_init() can be used to initialise (zero) a hrt_call structure to ensure safe usage. The hrt_call_every() interface calls this automatically. hrt_call_delay() can be used to delay a current callout by the given number of microseconds --- src/drivers/drv_hrt.h | 14 ++++++++++++++ src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca7..d130d68b3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); +/* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + /* * Initialise the HRT. */ diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a..dc28f446b 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { + hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -839,7 +840,12 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } hrt_call_enter(call); } } @@ -906,5 +912,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ -- cgit v1.2.3 From 70e56a3d54ec517d97b6f080badfab6c66c06f77 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:37 +1100 Subject: px4fmu2: enable SPI sensor DRDY pins --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 2527e4c14..c66c490a7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 -- cgit v1.2.3 From bc8cfc8d9d29650cc2ec7627cf5cedae0b5ed47d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 10 Dec 2013 11:21:27 +0100 Subject: Fix indendation in airspeed driver (no functional change) --- src/drivers/airspeed/airspeed.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 62c0d1f17..c341aa2c6 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - float _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; -- cgit v1.2.3 From 0349937a828392d50e736d32f4eec6242d65c9aa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 10:19:35 +1100 Subject: lsm303d: added detailed logging of accels on extremes this will log accel values and registers to /fs/microsd/lsm303d.log if any extreme values are seen --- src/drivers/lsm303d/lsm303d.cpp | 165 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 163 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 356569f99..6da684c10 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ #include #include #include +#include #include #include @@ -231,6 +233,16 @@ public: */ void print_registers(); + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -273,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -283,6 +296,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -478,11 +500,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -519,6 +548,9 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -628,6 +660,101 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR }; + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + ::ioctl(tfd, TONE_SET_ALARM, 4); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -646,6 +773,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -1495,6 +1623,18 @@ LSM303D::print_registers() printf("_reg7_expected=0x%02x\n", _reg7_expected); } +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", MAG_DEVICE_PATH), _parent(parent) @@ -1548,6 +1688,7 @@ void test(); void reset(); void info(); void regdump(); +void logging(); /** * Start the driver. @@ -1734,6 +1875,20 @@ regdump() exit(0); } +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1771,5 +1926,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } -- cgit v1.2.3 From 895dc3a2bbf172c7a2095d3a815cdd13d1388816 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:39:07 +1100 Subject: lsm303d: dump I2C control registers in regdump --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6da684c10..fc81c72a5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1574,6 +1574,8 @@ LSM303D::print_registers() const char *name; } regmap[] = { { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, { ADDR_STATUS_A, "STATUS_A" }, { ADDR_STATUS_M, "STATUS_M" }, { ADDR_CTRL_REG0, "CTRL_REG0" }, -- cgit v1.2.3 From b2b9665e4460ce25718509b7abb75c4b702c4c5f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:43:54 +1100 Subject: device: added register_class_devname() API this allows drivers to register generic device names for a device class, with automatic class instance handling --- src/drivers/device/cdev.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/drivers/device/device.h | 22 ++++++++++++++++++++++ 2 files changed, 58 insertions(+) (limited to 'src') diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a..7954ce5ab 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -108,6 +108,42 @@ CDev::~CDev() unregister_driver(_devname); } +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + int CDev::init() { diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c..0235f6284 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ -- cgit v1.2.3 From 5ee41bc0835df045593cba50d90ef004cfec3167 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:44:58 +1100 Subject: hmc5883: use register_class_devname() --- src/drivers/hmc5883/hmc5883.cpp | 35 +++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22ad301ff..d3b99ae66 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -155,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -322,6 +323,7 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -352,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -375,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -876,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1292,7 +1303,7 @@ test() int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1389,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1414,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From 5a88dc02a7a7e6386f3987794afd8e3881abe0b3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:10 +1100 Subject: l3gd20: use register_class_devname() --- src/drivers/l3gd20/l3gd20.cpp | 37 ++++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a27bc5280..78a086b11 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include #include +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -199,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -317,6 +320,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), @@ -346,6 +350,9 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); } @@ -365,17 +372,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -743,7 +746,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -922,7 +925,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -931,7 +934,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -963,10 +966,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -999,7 +1002,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From e334377e6c74f01a2bd75e435f911bb2dc29f401 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:28 +1100 Subject: lsm303d: use register_class_devname() --- src/drivers/lsm303d/lsm303d.cpp | 121 ++++++++++++++++++++-------------------- 1 file changed, 62 insertions(+), 59 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 13ef682b4..cb4b0b5d1 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,8 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -277,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -466,6 +466,8 @@ public: 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 init(); + protected: friend class LSM303D; @@ -473,6 +475,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -494,7 +499,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -544,6 +549,9 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ @@ -573,10 +581,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -584,53 +588,22 @@ LSM303D::init() reset(); - /* register the first instance as plain name, the 2nd as two and counting */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); - - if (mag_ret == OK) { - log("default mag device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; } - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - ret = OK; out: return ret; } @@ -1510,8 +1483,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1582,8 +1557,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1673,13 +1650,39 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", "/dev/lsm303d_mag"), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1739,7 +1742,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1858,7 +1861,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1869,7 +1872,7 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); -- cgit v1.2.3 From b55403c551070d681b9b438a0b52deb5832adc8f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:39 +1100 Subject: mpu6000: use register_class_devname() --- src/drivers/mpu6000/mpu6000.cpp | 114 +++++++++++++++++++++++++--------------- 1 file changed, 71 insertions(+), 43 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce0010752..6145a5117 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -211,16 +211,17 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; math::LowPassFilter2p _accel_filter_x; @@ -349,12 +350,17 @@ public: 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 init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -370,12 +376,13 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -420,8 +427,13 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); } int @@ -466,38 +478,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -677,6 +674,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; @@ -694,12 +693,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -771,6 +770,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; @@ -1180,9 +1181,6 @@ MPU6000::measure() if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1287,10 +1285,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1301,19 +1300,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), - _parent(parent) + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void -- cgit v1.2.3 From 1bac7e7f8b073a7e5cee12570e42694988df1abc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:31 +1100 Subject: l3gd20: close fds before exit --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 78a086b11..910131c6a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -942,6 +942,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -990,6 +992,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -1013,6 +1017,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } -- cgit v1.2.3 From 038ec194ae55c94d566a62f9e6e7dbcc0e0e7399 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:43 +1100 Subject: lsm303d: close fds before exit --- src/drivers/lsm303d/lsm303d.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index cb4b0b5d1..4d7c8f05a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1770,6 +1770,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1851,6 +1853,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1872,6 +1877,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { @@ -1882,6 +1889,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } -- cgit v1.2.3 From 39b40e41c2126cb2aeba586a57333ebb2aa3c2a6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:54 +1100 Subject: mpu6000: close fds before exit --- src/drivers/mpu6000/mpu6000.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6145a5117..ff921274b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1405,6 +1405,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1508,6 +1510,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } -- cgit v1.2.3 From 8744aa7536d2c52417855bd28ef589b72a42e5de Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 10:03:50 +1100 Subject: ms5611: check for all zero in the prom when SPI CLK fails we get all zero data --- src/drivers/ms5611/ms5611_spi.cpp | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e9dff5a8b..bc4074c55 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -236,9 +236,12 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } @@ -247,6 +250,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { debug("crc failed"); } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } return ret; } -- cgit v1.2.3 From 2b491a7954b8bbcca044790f314c78e0c91019df Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 09:13:38 +1100 Subject: mpu6000: treat all zero data from mpu6k as bad --- src/drivers/mpu6000/mpu6000.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ff921274b..c95d11c83 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -223,6 +223,7 @@ private: perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -384,6 +385,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -434,6 +436,7 @@ MPU6000::~MPU6000() perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -1013,9 +1016,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1028,9 +1029,7 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1195,6 +1194,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ -- cgit v1.2.3 From 893d66d9613c699c9e891e23bc78a61fcc5ad7b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 14:03:16 +1100 Subject: l3gd20: added rescheduling and error checking --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 910131c6a..1077eb43b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -209,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -325,6 +327,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) @@ -355,6 +359,8 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -746,7 +752,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -776,6 +782,17 @@ L3GD20::measure_trampoline(void *arg) void L3GD20::measure() { +#ifdef GPIO_EXTI_GYRO_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -798,6 +815,16 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#ifdef GPIO_EXTI_GYRO_DRDY + if (raw_report.status & 0xF != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) -- cgit v1.2.3 From b3f4b0a240bfb9d7dfaafd78d0a6bbca1c429f60 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 15:09:36 +1100 Subject: drv_hrt: added note on why an uninitialised hrt_call is safe --- src/drivers/stm32/drv_hrt.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index dc28f446b..1bd251bc2 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,7 +720,6 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -734,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); -- cgit v1.2.3 From 1b1aa0edea9214d5e7bab2db634b4fdbf8ad3e95 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:52:31 +1100 Subject: lsm303d: use DRDY pins to automatically reschedule measurements this prevents double reads of sensor data, and missing samples from the accel --- src/drivers/lsm303d/lsm303d.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4d7c8f05a..b13ca9f44 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -287,6 +287,7 @@ private: perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -507,6 +508,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -635,6 +637,8 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -1416,6 +1420,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); -- cgit v1.2.3 From 93f3398dfedd787419f54294adc6c92c30e282b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 10:06:43 +1100 Subject: lsm303d: added 'lsm303d regdump' command useful for diagnosing issues --- src/drivers/lsm303d/lsm303d.cpp | 53 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 52 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 60601e22c..c21a25522 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -201,6 +201,11 @@ public: */ void print_info(); + /** + * dump register values + */ + void print_registers(); + protected: virtual int probe(); @@ -1380,6 +1385,30 @@ LSM303D::print_info() _mag_reports->print_info("mag reports"); } +void +LSM303D::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + }; + for (uint8_t i=0; iprint_registers(); + + exit(0); +} + } // namespace @@ -1634,5 +1679,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) lsm303d::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "regdump")) + lsm303d::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } -- cgit v1.2.3 From af049f7cf8b261aaa85a659537d75c1476da40f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:17:53 +1100 Subject: lsm303d: define some more register addresses --- src/drivers/lsm303d/lsm303d.cpp | 51 ++++++++++++++++++++++++++++++----------- 1 file changed, 38 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c21a25522..de227974e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -82,19 +82,24 @@ static const int ERROR = -1; /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F -#define WHO_I_AM 0x49 - -#define ADDR_OUT_L_T 0x05 -#define ADDR_OUT_H_T 0x06 -#define ADDR_STATUS_M 0x07 -#define ADDR_OUT_X_L_M 0x08 -#define ADDR_OUT_X_H_M 0x09 -#define ADDR_OUT_Y_L_M 0x0A -#define ADDR_OUT_Y_H_M 0x0B -#define ADDR_OUT_Z_L_M 0x0C -#define ADDR_OUT_Z_H_M 0x0D - -#define ADDR_OUT_TEMP_A 0x26 +#define WHO_I_AM 0x49 + +#define ADDR_OUT_TEMP_L 0x05 +#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_STATUS_M 0x07 +#define ADDR_OUT_X_L_M 0x08 +#define ADDR_OUT_X_H_M 0x09 +#define ADDR_OUT_Y_L_M 0x0A +#define ADDR_OUT_Y_H_M 0x0B +#define ADDR_OUT_Z_L_M 0x0C +#define ADDR_OUT_Z_H_M 0x0D + +#define ADDR_INT_CTRL_M 0x12 +#define ADDR_INT_SRC_M 0x13 +#define ADDR_REFERENCE_X 0x1c +#define ADDR_REFERENCE_Y 0x1d +#define ADDR_REFERENCE_Z 0x1e + #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 @@ -112,6 +117,26 @@ static const int ERROR = -1; #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 +#define ADDR_FIFO_CTRL 0x2e +#define ADDR_FIFO_SRC 0x2f + +#define ADDR_IG_CFG1 0x30 +#define ADDR_IG_SRC1 0x31 +#define ADDR_IG_THS1 0x32 +#define ADDR_IG_DUR1 0x33 +#define ADDR_IG_CFG2 0x34 +#define ADDR_IG_SRC2 0x35 +#define ADDR_IG_THS2 0x36 +#define ADDR_IG_DUR2 0x37 +#define ADDR_CLICK_CFG 0x38 +#define ADDR_CLICK_SRC 0x39 +#define ADDR_CLICK_THS 0x3a +#define ADDR_TIME_LIMIT 0x3b +#define ADDR_TIME_LATENCY 0x3c +#define ADDR_TIME_WINDOW 0x3d +#define ADDR_ACT_THS 0x3e +#define ADDR_ACT_DUR 0x3f + #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -- cgit v1.2.3 From 415417196bcafdafa6aafa8109487ed0ed18cab6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Oct 2013 18:12:50 +1100 Subject: lsm303d: print more registers in "lsm303d regdump" --- src/drivers/lsm303d/lsm303d.cpp | 55 ++++++++++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index de227974e..91f1fe005 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1417,21 +1417,54 @@ LSM303D::print_registers() uint8_t reg; const char *name; } regmap[] = { - { ADDR_WHO_AM_I, "WHO_AM_I" }, - { ADDR_STATUS_A, "STATUS_A" }, - { ADDR_STATUS_M, "STATUS_M" }, - { ADDR_CTRL_REG0, "CTRL_REG0" }, - { ADDR_CTRL_REG1, "CTRL_REG1" }, - { ADDR_CTRL_REG2, "CTRL_REG2" }, - { ADDR_CTRL_REG3, "CTRL_REG3" }, - { ADDR_CTRL_REG4, "CTRL_REG4" }, - { ADDR_CTRL_REG5, "CTRL_REG5" }, - { ADDR_CTRL_REG6, "CTRL_REG6" }, - { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_WHO_AM_I, "WHO_AM_I" }, + { ADDR_STATUS_A, "STATUS_A" }, + { ADDR_STATUS_M, "STATUS_M" }, + { ADDR_CTRL_REG0, "CTRL_REG0" }, + { ADDR_CTRL_REG1, "CTRL_REG1" }, + { ADDR_CTRL_REG2, "CTRL_REG2" }, + { ADDR_CTRL_REG3, "CTRL_REG3" }, + { ADDR_CTRL_REG4, "CTRL_REG4" }, + { ADDR_CTRL_REG5, "CTRL_REG5" }, + { ADDR_CTRL_REG6, "CTRL_REG6" }, + { ADDR_CTRL_REG7, "CTRL_REG7" }, + { ADDR_OUT_TEMP_L, "TEMP_L" }, + { ADDR_OUT_TEMP_H, "TEMP_H" }, + { ADDR_INT_CTRL_M, "INT_CTRL_M" }, + { ADDR_INT_SRC_M, "INT_SRC_M" }, + { ADDR_REFERENCE_X, "REFERENCE_X" }, + { ADDR_REFERENCE_Y, "REFERENCE_Y" }, + { ADDR_REFERENCE_Z, "REFERENCE_Z" }, + { ADDR_OUT_X_L_A, "ACCEL_XL" }, + { ADDR_OUT_X_H_A, "ACCEL_XH" }, + { ADDR_OUT_Y_L_A, "ACCEL_YL" }, + { ADDR_OUT_Y_H_A, "ACCEL_YH" }, + { ADDR_OUT_Z_L_A, "ACCEL_ZL" }, + { ADDR_OUT_Z_H_A, "ACCEL_ZH" }, + { ADDR_FIFO_CTRL, "FIFO_CTRL" }, + { ADDR_FIFO_SRC, "FIFO_SRC" }, + { ADDR_IG_CFG1, "IG_CFG1" }, + { ADDR_IG_SRC1, "IG_SRC1" }, + { ADDR_IG_THS1, "IG_THS1" }, + { ADDR_IG_DUR1, "IG_DUR1" }, + { ADDR_IG_CFG2, "IG_CFG2" }, + { ADDR_IG_SRC2, "IG_SRC2" }, + { ADDR_IG_THS2, "IG_THS2" }, + { ADDR_IG_DUR2, "IG_DUR2" }, + { ADDR_CLICK_CFG, "CLICK_CFG" }, + { ADDR_CLICK_SRC, "CLICK_SRC" }, + { ADDR_CLICK_THS, "CLICK_THS" }, + { ADDR_TIME_LIMIT, "TIME_LIMIT" }, + { ADDR_TIME_LATENCY,"TIME_LATENCY" }, + { ADDR_TIME_WINDOW, "TIME_WINDOW" }, + { ADDR_ACT_THS, "ACT_THS" }, + { ADDR_ACT_DUR, "ACT_DUR" } }; for (uint8_t i=0; i Date: Wed, 30 Oct 2013 09:45:58 +1100 Subject: SPI: added set_frequency() API this allows the bus speed to be changed on the fly by device drivers. This is needed for the MPU6000 --- src/drivers/device/spi.cpp | 6 ++++++ src/drivers/device/spi.h | 11 +++++++++++ 2 files changed, 17 insertions(+) (limited to 'src') diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fa6b78d64..fed6c312c 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -181,4 +181,10 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return OK; } +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9103dca2e..299575dc5 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency); + /** * Locking modes supported by the driver. */ -- cgit v1.2.3 From 97af3d22040e67520a835102684a1b2a9575aaaa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 09:46:52 +1100 Subject: mpu6000: change bus speed based on registers being accessed this ensures we follow the datasheet requirement of 1MHz for general registers and up to 20MHz for sensor and int status registers --- src/drivers/mpu6000/mpu6000.cpp | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 70359110e..6bfa583fb 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -161,6 +161,14 @@ #define MPU6000_ONE_G 9.80665f +/* + the MPU6000 can only handle high SPI bus speeds on the sensor and + interrupt status registers. All other registers have a maximum 1MHz + SPI speed + */ +#define MPU6000_LOW_BUS_SPEED 1000*1000 +#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -351,7 +359,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), + SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -991,6 +999,9 @@ MPU6000::read_reg(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; @@ -1003,6 +1014,9 @@ MPU6000::read_reg16(unsigned reg) cmd[0] = reg | DIR_READ; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, cmd, sizeof(cmd)); return (uint16_t)(cmd[1] << 8) | cmd[2]; @@ -1016,6 +1030,9 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); + transfer(cmd, nullptr, sizeof(cmd)); } @@ -1139,6 +1156,10 @@ MPU6000::measure() * Fetch the full set of measurements from the MPU6000 in one pass. */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; -- cgit v1.2.3 From 50d5241985792d829833f287f2fc1936d8caef84 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Oct 2013 15:27:48 +1100 Subject: px4io: moved blue heartbeat LED to main loop this allows us to tell if the main loop is running by looking for a blinking blue LED --- src/modules/px4iofirmware/px4io.c | 13 +++++++++++++ src/modules/px4iofirmware/safety.c | 16 +--------------- 2 files changed, 14 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index ff9eecd74..cd9bd197b 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -117,6 +117,13 @@ show_debug_messages(void) } } +static void +heartbeat_blink(void) +{ + static bool heartbeat = false; + LED_BLUE(heartbeat = !heartbeat); +} + int user_start(int argc, char *argv[]) { @@ -201,6 +208,7 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; + uint64_t last_heartbeat_time = 0; for (;;) { /* track the rate at which the loop is running */ @@ -216,6 +224,11 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); + if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 95335f038..cdb54a80a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -77,7 +77,6 @@ static unsigned blink_counter = 0; static bool safety_button_pressed; static void safety_check_button(void *arg); -static void heartbeat_blink(void *arg); static void failsafe_blink(void *arg); void @@ -86,9 +85,6 @@ safety_init(void) /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); - /* arrange for the heartbeat handler to be called at 4Hz */ - hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL); - /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -163,16 +159,6 @@ safety_check_button(void *arg) } } -static void -heartbeat_blink(void *arg) -{ - static bool heartbeat = false; - - /* XXX add flags here that need to be frobbed by various loops */ - - LED_BLUE(heartbeat = !heartbeat); -} - static void failsafe_blink(void *arg) { @@ -192,4 +178,4 @@ failsafe_blink(void *arg) } LED_AMBER(failsafe); -} \ No newline at end of file +} -- cgit v1.2.3 From cdaafff6e45b2d68d20248fd00ec157b47ff6f78 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 10:19:35 +1100 Subject: lsm303d: added detailed logging of accels on extremes this will log accel values and registers to /fs/microsd/lsm303d.log if any extreme values are seen --- src/drivers/lsm303d/lsm303d.cpp | 165 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 163 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 91f1fe005..e7869527c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -63,6 +64,7 @@ #include #include #include +#include #include #include @@ -231,6 +233,16 @@ public: */ void print_registers(); + /** + * toggle logging + */ + void toggle_logging(); + + /** + * check for extreme accel values + */ + void check_extremes(const accel_report *arb); + protected: virtual int probe(); @@ -273,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; + perf_counter_t _extreme_values; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -283,6 +296,15 @@ private: uint8_t _reg7_expected; uint8_t _reg1_expected; + // accel logging + int _accel_log_fd; + bool _accel_logging_enabled; + uint64_t _last_extreme_us; + uint64_t _last_log_us; + uint64_t _last_log_sync_us; + uint64_t _last_log_reg_us; + uint64_t _last_log_alarm_us; + /** * Start automatic measurement. */ @@ -473,11 +495,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), + _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), - _reg7_expected(0) + _reg7_expected(0), + _accel_log_fd(-1), + _accel_logging_enabled(false), + _last_log_us(0), + _last_log_sync_us(0), + _last_log_reg_us(0), + _last_log_alarm_us(0) { // enable debug() calls _debug_enabled = true; @@ -514,6 +543,9 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); + perf_free(_reg1_resets); + perf_free(_reg7_resets); + perf_free(_extreme_values); } int @@ -602,6 +634,101 @@ LSM303D::probe() return -EIO; } +#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" + +/** + check for extreme accelerometer values and log to a file on the SD card + */ +void +LSM303D::check_extremes(const accel_report *arb) +{ + const float extreme_threshold = 30; + bool is_extreme = (fabsf(arb->x) > extreme_threshold && + fabsf(arb->y) > extreme_threshold && + fabsf(arb->z) > extreme_threshold); + if (is_extreme) { + perf_count(_extreme_values); + // force accel logging on if we see extreme values + _accel_logging_enabled = true; + } + + if (! _accel_logging_enabled) { + // logging has been disabled by user, close + if (_accel_log_fd != -1) { + ::close(_accel_log_fd); + _accel_log_fd = -1; + } + return; + } + if (_accel_log_fd == -1) { + // keep last 10 logs + ::unlink(ACCEL_LOGFILE ".9"); + for (uint8_t i=8; i>0; i--) { + uint8_t len = strlen(ACCEL_LOGFILE)+3; + char log1[len], log2[len]; + snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); + snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); + ::rename(log1, log2); + } + ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); + + // open the new logfile + _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); + if (_accel_log_fd == -1) { + return; + } + } + + uint64_t now = hrt_absolute_time(); + // log accels at 1Hz + if (now - _last_log_us > 1000*1000) { + _last_log_us = now; + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + (unsigned long long)arb->timestamp, + arb->x, arb->y, arb->z, + (int)arb->x_raw, + (int)arb->y_raw, + (int)arb->z_raw); + } + + // log registers at 10Hz when we have extreme values, or 0.5 Hz without + if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + (now - _last_log_reg_us > 10*1000*1000)) { + _last_log_reg_us = now; + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR }; + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + for (uint8_t i=0; i 10*1000*1000) { + _last_log_sync_us = now; + ::fsync(_accel_log_fd); + } + + // play alarm every 10s if we have had an extreme value + if (perf_event_count(_extreme_values) != 0 && + (now - _last_log_alarm_us > 10*1000*1000)) { + _last_log_alarm_us = now; + int tfd = ::open(TONEALARM_DEVICE_PATH, 0); + if (tfd != -1) { + ::ioctl(tfd, TONE_SET_ALARM, 4); + ::close(tfd); + } + } +} + ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { @@ -620,6 +747,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { + check_extremes(arb); ret += sizeof(*arb); arb++; } @@ -1467,6 +1595,18 @@ LSM303D::print_registers() printf("_reg7_expected=0x%02x\n", _reg7_expected); } +void +LSM303D::toggle_logging() +{ + if (! _accel_logging_enabled) { + _accel_logging_enabled = true; + printf("Started logging to %s\n", ACCEL_LOGFILE); + } else { + _accel_logging_enabled = false; + printf("Stopped logging\n"); + } +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", MAG_DEVICE_PATH), _parent(parent) @@ -1520,6 +1660,7 @@ void test(); void reset(); void info(); void regdump(); +void logging(); /** * Start the driver. @@ -1706,6 +1847,20 @@ regdump() exit(0); } +/** + * toggle logging + */ +void +logging() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->toggle_logging(); + + exit(0); +} + } // namespace @@ -1743,5 +1898,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(argv[1], "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * dump device registers + */ + if (!strcmp(argv[1], "logging")) + lsm303d::logging(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); } -- cgit v1.2.3 From 671447ce2c94ad523ea9dc636c5066db98831d87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 11:12:08 +1100 Subject: lsm303d: fixed TEMP_H register define --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index e7869527c..bbf70c154 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -87,7 +87,7 @@ static const int ERROR = -1; #define WHO_I_AM 0x49 #define ADDR_OUT_TEMP_L 0x05 -#define ADDR_OUT_TEMP_H 0x05 +#define ADDR_OUT_TEMP_H 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 -- cgit v1.2.3 From 5ef91d694b94c297bfecae297b2c933561e0ac16 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 Nov 2013 13:39:59 +1100 Subject: lsm303d: log mag regs too --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bbf70c154..c20c820aa 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -703,7 +703,9 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR }; + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Fri, 15 Nov 2013 14:12:24 +1100 Subject: lsm303d: always log first ARB and REG values --- src/drivers/lsm303d/lsm303d.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c20c820aa..6d7b3df50 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -681,7 +681,8 @@ LSM303D::check_extremes(const accel_report *arb) uint64_t now = hrt_absolute_time(); // log accels at 1Hz - if (now - _last_log_us > 1000*1000) { + if (_last_log_us == 0 || + now - _last_log_us > 1000*1000) { _last_log_us = now; ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", (unsigned long long)arb->timestamp, @@ -692,7 +693,8 @@ LSM303D::check_extremes(const accel_report *arb) } // log registers at 10Hz when we have extreme values, or 0.5 Hz without - if ((is_extreme && (now - _last_log_reg_us > 500*1000)) || + if (_last_log_reg_us == 0 || + (is_extreme && (now - _last_log_reg_us > 500*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, -- cgit v1.2.3 From ea33a19c8f5b109fd9ba35603b0af56dddef3708 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 16 Nov 2013 18:36:48 +1100 Subject: lsm303d: show regs at both high and low bus speed on error --- src/drivers/lsm303d/lsm303d.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6d7b3df50..2d150cdba 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -708,11 +708,20 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Sat, 16 Nov 2013 18:37:26 +1100 Subject: lsm303d: zero-fill register reads --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2d150cdba..bfa129364 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1123,6 +1123,7 @@ LSM303D::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); -- cgit v1.2.3 From b927974a976a86271826a04f0020be01007368ad Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Nov 2013 20:56:15 +1100 Subject: FMUv2: added support for MPU6000 on v2.4 board --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 13 +++++++++++++ 2 files changed, 15 insertions(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ec8dde499..534394274 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -85,11 +85,13 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 +#define PX4_SPIDEV_MPU 4 /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 09838d02f..2527e4c14 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, * required for some peripheral @@ -81,6 +82,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); #endif #ifdef CONFIG_STM32_SPI2 @@ -99,6 +101,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_ACCEL_MAG: @@ -106,6 +109,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; case PX4_SPIDEV_BARO: @@ -113,6 +117,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_MPU: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; default: -- cgit v1.2.3 From 3ce14497a118fc6a10c475704dfa5dbdd9669476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:28:42 +1100 Subject: FMUv2: added define for MPU DRDY pin --- src/drivers/boards/px4fmu-v2/board_config.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 534394274..9f66d195d 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -79,6 +79,7 @@ __BEGIN_DECLS #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) #define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN1) #define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN4) +#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -- cgit v1.2.3 From 44b2543d2d968a61b2b7cbef6d4409dcdf5c9174 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:02 +1100 Subject: FMUv2: set MPU6000 CS as initially de-selected --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index ae2a645f7..31ad60bd3 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -279,6 +279,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false); SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false); SPI_SELECT(spi1, PX4_SPIDEV_BARO, false); + SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); message("[boot] Successfully initialized SPI port 1\n"); -- cgit v1.2.3 From 9a169d8ef453aea1729b76940b10733591c4c6d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:29:33 +1100 Subject: l3gd20: added I2C disable based on method from ST engineering support --- src/drivers/l3gd20/l3gd20.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 8f5674823..31e38fbd9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -218,6 +218,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -574,6 +579,7 @@ L3GD20::read_reg(unsigned reg) uint8_t cmd[2]; cmd[0] = reg | DIR_READ; + cmd[1] = 0; transfer(cmd, cmd, sizeof(cmd)); @@ -699,9 +705,19 @@ L3GD20::stop() hrt_cancel(&_call); } +void +L3GD20::disable_i2c(void) +{ + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); +} + void L3GD20::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ @@ -753,6 +769,7 @@ L3GD20::measure() perf_begin(_sample_perf); /* fetch data from the sensor */ + memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -- cgit v1.2.3 From 7c9d92a5d64ee4b2282139a49e189df434ec00f1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:04 +1100 Subject: lsm303d: added I2C disable based on method from ST engineering support --- src/drivers/lsm303d/lsm303d.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index bfa129364..da981989c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -322,6 +322,11 @@ private: */ void reset(); + /** + * disable I2C on the chip + */ + void disable_i2c(); + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -595,9 +600,25 @@ out: return ret; } +void +LSM303D::disable_i2c(void) +{ + uint8_t a = read_reg(0x02); + write_reg(0x02, (0x10 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xF7 & a)); + a = read_reg(0x15); + write_reg(0x15, (0x80 | a)); + a = read_reg(0x02); + write_reg(0x02, (0xE7 & a)); +} + void LSM303D::reset() { + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + /* enable accel*/ _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; write_reg(ADDR_CTRL_REG1, _reg1_expected); -- cgit v1.2.3 From a2b31118cb4dc01174c4c3436c29d5850d116441 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 28 Nov 2013 20:30:48 +1100 Subject: lsm303d: get cleaner logic traces by gathering all regs more regularly --- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++++++++++---------------------- 1 file changed, 19 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index da981989c..0c0aa0ff5 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -713,36 +713,32 @@ LSM303D::check_extremes(const accel_report *arb) (int)arb->z_raw); } + const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, + ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, + ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, + ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, + ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, + ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, + ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, + ADDR_ACT_THS, ADDR_ACT_DUR, + ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + uint8_t regval[sizeof(reglist)]; + for (uint8_t i=0; i 500*1000)) || + (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; - ::dprintf(_accel_log_fd, "FREG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Thu, 28 Nov 2013 20:31:28 +1100 Subject: lsm303d: cleanup logic traces by pre-zeroing all transfers --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 0c0aa0ff5..576bc184e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,6 +1437,7 @@ LSM303D::measure() perf_begin(_accel_sample_perf); /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); @@ -1514,6 +1515,7 @@ LSM303D::mag_measure() perf_begin(_mag_sample_perf); /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); -- cgit v1.2.3 From aeba9e5c1e59b016b6ce707852490714db0544e2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:31:51 +1100 Subject: FMUv2: change CS pins to 2MHz this gives cleaner traces --- src/drivers/boards/px4fmu-v2/board_config.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 9f66d195d..586661b58 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -82,11 +82,11 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ -#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) -#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 -- cgit v1.2.3 From 51a1ad48c5734f259bc8c90b2b5f5626b83cbdbb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 30 Nov 2013 16:33:32 +1100 Subject: FMUv2: don't config ADC pins that are now used for MPU6k CS and other uses --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 31ad60bd3..269ec238e 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -215,9 +215,9 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */ stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN10); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN11); /* unrouted */ - stm32_configgpio(GPIO_ADC1_IN12); /* unrouted */ + // stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + // stm32_configgpio(GPIO_ADC1_IN11); /* unused */ + // stm32_configgpio(GPIO_ADC1_IN12); /* used by MPU6000 CS */ stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ -- cgit v1.2.3 From c46ab017e12f5ccbe0f22ec77f90d8a8e28a8c63 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 12:06:05 +1100 Subject: lsm303d: make log distinctive with i2c disable included --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 576bc184e..5c9a87eaa 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -734,7 +734,7 @@ LSM303D::check_extremes(const accel_report *arb) (is_extreme && (now - _last_log_reg_us > 250*1000)) || (now - _last_log_reg_us > 10*1000*1000)) { _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "REG %llu", (unsigned long long)hrt_absolute_time()); + ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); for (uint8_t i=0; i Date: Wed, 4 Dec 2013 15:50:41 +1100 Subject: lsm303d: changed tones for accel fail to 3 tones distinct tones for init fail, post-boot fail and recovery --- src/drivers/lsm303d/lsm303d.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 5c9a87eaa..44cb46912 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -664,6 +664,7 @@ void LSM303D::check_extremes(const accel_report *arb) { const float extreme_threshold = 30; + static bool boot_ok = false; bool is_extreme = (fabsf(arb->x) > extreme_threshold && fabsf(arb->y) > extreme_threshold && fabsf(arb->z) > extreme_threshold); @@ -671,7 +672,9 @@ LSM303D::check_extremes(const accel_report *arb) perf_count(_extreme_values); // force accel logging on if we see extreme values _accel_logging_enabled = true; - } + } else { + boot_ok = true; + } if (! _accel_logging_enabled) { // logging has been disabled by user, close @@ -705,15 +708,16 @@ LSM303D::check_extremes(const accel_report *arb) if (_last_log_us == 0 || now - _last_log_us > 1000*1000) { _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d\r\n", + ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", (unsigned long long)arb->timestamp, arb->x, arb->y, arb->z, (int)arb->x_raw, (int)arb->y_raw, - (int)arb->z_raw); + (int)arb->z_raw, + (unsigned)boot_ok); } - const uint8_t reglist[] = { ADDR_WHO_AM_I, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, + const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, @@ -723,7 +727,7 @@ LSM303D::check_extremes(const accel_report *arb) ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, ADDR_ACT_THS, ADDR_ACT_DUR, ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M}; + ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; uint8_t regval[sizeof(reglist)]; for (uint8_t i=0; i Date: Fri, 1 Nov 2013 08:11:58 +1100 Subject: lsm303d: init filter to 773 Hz --- src/drivers/lsm303d/lsm303d.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 44cb46912..a38524c22 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,7 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -- cgit v1.2.3 From 0a83772c0d8b4b600245590d571f679a6894e75f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 16:12:25 +1100 Subject: l3gd20: use highest possible on-chip filter bandwidth this allows the software filter to do its job properly --- src/drivers/l3gd20/l3gd20.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 31e38fbd9..103b26ac5 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -92,9 +92,12 @@ static const int ERROR = -1; #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */ /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) -#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) +#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 @@ -659,16 +662,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_25HZ; + bits |= RATE_190HZ_LP_70HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_20HZ; + bits |= RATE_380HZ_LP_100HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_30HZ; - + bits |= RATE_760HZ_LP_100HZ; } else { return -EINVAL; } @@ -732,7 +734,7 @@ L3GD20::reset() * callback fast enough to not miss data. */ write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE); - set_samplerate(L3GD20_DEFAULT_RATE); + set_samplerate(0); // 760Hz set_range(L3GD20_DEFAULT_RANGE_DPS); set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ); -- cgit v1.2.3 From 24a243843ef771273c81536c0bd18c5d70c010f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Dec 2013 22:54:02 +1100 Subject: lsm303d/l3gd20: change filters to 50Hz analog on-chip filters after discussion with Leonard these analog on-chip filters should be at 50Hz --- src/drivers/l3gd20/l3gd20.cpp | 11 ++++++++--- src/drivers/lsm303d/lsm303d.cpp | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 103b26ac5..17b53bfa9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -93,10 +93,15 @@ static const int ERROR = -1; /* keep lowpass low to avoid noise issues */ #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) +#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4)) +#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4)) #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4)) +#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4)) +#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4)) #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define ADDR_CTRL_REG2 0x21 @@ -662,15 +667,15 @@ L3GD20::set_samplerate(unsigned frequency) } else if (frequency <= 200) { _current_rate = 190; - bits |= RATE_190HZ_LP_70HZ; + bits |= RATE_190HZ_LP_50HZ; } else if (frequency <= 400) { _current_rate = 380; - bits |= RATE_380HZ_LP_100HZ; + bits |= RATE_380HZ_LP_50HZ; } else if (frequency <= 800) { _current_rate = 760; - bits |= RATE_760HZ_LP_100HZ; + bits |= RATE_760HZ_LP_50HZ; } else { return -EINVAL; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a38524c22..701947b98 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -631,7 +631,12 @@ LSM303D::reset() accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); - accel_set_onchip_lowpass_filter_bandwidth(0); // this gives 773Hz + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); -- cgit v1.2.3 From 513d014f03831111a1b9f9af293e253f7d5218a5 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Dec 2013 09:06:53 +1100 Subject: l3gd20: added retries to disable_i2c() --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 17b53bfa9..176ef648b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -715,8 +715,16 @@ L3GD20::stop() void L3GD20::disable_i2c(void) { - uint8_t a = read_reg(0x05); - write_reg(0x05, (0x20 | a)); + uint8_t retries = 10; + while (retries--) { + // add retries + uint8_t a = read_reg(0x05); + write_reg(0x05, (0x20 | a)); + if (read_reg(0x05) == (a | 0x20)) { + return; + } + } + debug("FAILED TO DISABLE I2C"); } void -- cgit v1.2.3 From 4956feffdfc8459c5f27e471b50978db29f23c07 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:00 +1100 Subject: drv_hrt: added hrt_call_init() and hrt_call_delay() APIs hrt_call_init() can be used to initialise (zero) a hrt_call structure to ensure safe usage. The hrt_call_every() interface calls this automatically. hrt_call_delay() can be used to delay a current callout by the given number of microseconds --- src/drivers/drv_hrt.h | 14 ++++++++++++++ src/drivers/stm32/drv_hrt.c | 19 ++++++++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 8a99eeca7..d130d68b3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -141,6 +141,20 @@ __EXPORT extern bool hrt_called(struct hrt_call *entry); */ __EXPORT extern void hrt_cancel(struct hrt_call *entry); +/* + * initialise a hrt_call structure + */ +__EXPORT extern void hrt_call_init(struct hrt_call *entry); + +/* + * delay a hrt_call_every() periodic call by the given number of + * microseconds. This should be called from within the callout to + * cause the callout to be re-scheduled for a later time. The periodic + * callouts will then continue from that new base time at the + * previously specified period. + */ +__EXPORT extern void hrt_call_delay(struct hrt_call *entry, hrt_abstime delay); + /* * Initialise the HRT. */ diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index e79d7e10a..dc28f446b 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,6 +720,7 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { + hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -839,7 +840,12 @@ hrt_call_invoke(void) /* if the callout has a non-zero period, it has to be re-entered */ if (call->period != 0) { - call->deadline = deadline + call->period; + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } hrt_call_enter(call); } } @@ -906,5 +912,16 @@ hrt_latency_update(void) latency_counters[index]++; } +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} #endif /* HRT_TIMER */ -- cgit v1.2.3 From 0e97c288bbe5acde6358e10237d1bfb5e522ee19 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:51:37 +1100 Subject: px4fmu2: enable SPI sensor DRDY pins --- src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 2527e4c14..c66c490a7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -83,6 +83,11 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); + stm32_configgpio(GPIO_EXTI_MPU_DRDY); #endif #ifdef CONFIG_STM32_SPI2 -- cgit v1.2.3 From 30ff61fa90179af00609738da6bd5365872d4f61 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:52:31 +1100 Subject: lsm303d: use DRDY pins to automatically reschedule measurements this prevents double reads of sensor data, and missing samples from the accel --- src/drivers/lsm303d/lsm303d.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 701947b98..6010a2ad7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -286,6 +286,7 @@ private: perf_counter_t _reg7_resets; perf_counter_t _reg1_resets; perf_counter_t _extreme_values; + perf_counter_t _accel_reschedules; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -501,6 +502,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), + _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -627,6 +629,8 @@ LSM303D::reset() _reg7_expected = REG7_CONT_MODE_M; write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -1430,6 +1434,14 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + return; + } if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { perf_count(_reg1_resets); reset(); -- cgit v1.2.3 From 7e309414758d2c526da7ef3bcab7bf75779f6950 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:53:25 +1100 Subject: ms5611: change bus speed to 5MHz this gives 5MHz SPI bus speed (by asking for 6MHz due to timer granularity). Tests with a logic analyser show that the ms5611 is actually more reliable at 5MHz than lower speeds --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e547c913b..33e5be940 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 2000000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), _prom(prom_buf) { } -- cgit v1.2.3 From bc6ddb971fa23b88679ea8201a8205be3c08e90c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:54:58 +1100 Subject: ms5611: removed unused variable --- src/drivers/ms5611/ms5611_spi.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 33e5be940..8dd89120d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,7 +134,6 @@ int MS5611_SPI::init() { int ret; - irqstate_t flags; ret = SPI::init(); if (ret != OK) { -- cgit v1.2.3 From 0456ee2364600ba6b5a9f109c7464a71579e7d58 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Dec 2013 20:55:10 +1100 Subject: ms5611: give cleaner SPI traces this makes logic traces cleaner by zeroing extra bytes written --- src/drivers/ms5611/ms5611_spi.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 8dd89120d..e9dff5a8b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -166,10 +166,9 @@ MS5611_SPI::read(unsigned offset, void *data, unsigned count) uint8_t b[4]; uint32_t w; } *cvt = (_cvt *)data; - uint8_t buf[4]; + uint8_t buf[4] = { 0 | DIR_WRITE, 0, 0, 0 }; /* read the most recent measurement */ - buf[0] = 0 | DIR_WRITE; int ret = _transfer(&buf[0], &buf[0], sizeof(buf)); if (ret == OK) { @@ -240,18 +239,21 @@ MS5611_SPI::_read_prom() for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } /* calculate CRC and return success/failure accordingly */ - return ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + int ret = ms5611::crc4(&_prom.c[0]) ? OK : -EIO; + if (ret != OK) { + debug("crc failed"); + } + return ret; } uint16_t MS5611_SPI::_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; _transfer(cmd, cmd, sizeof(cmd)); -- cgit v1.2.3 From 3d27dd7246c660bb0a00caae3cd0b0e66bfa6467 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:34:25 +0100 Subject: Made all usual suspects default to their custom names and only register the default name if its not already taken by someone else --- src/drivers/hmc5883/hmc5883.cpp | 7 ++++--- src/drivers/l3gd20/l3gd20.cpp | 7 +++++++ src/drivers/lsm303d/lsm303d.cpp | 33 ++++++++++++++++++++++++--------- src/drivers/mpu6000/mpu6000.cpp | 33 +++++++++++++++++++++++++-------- 4 files changed, 60 insertions(+), 20 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5f0ce4ff8..22ad301ff 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -77,6 +77,7 @@ */ #define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 +#define HMC5883L_DEVICE_PATH "/dev/hmc5883" /* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */ #define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */ @@ -315,7 +316,7 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : - I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), + I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ @@ -1256,7 +1257,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -1288,7 +1289,7 @@ test() ssize_t sz; int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 176ef648b..a27bc5280 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -365,6 +365,13 @@ L3GD20::init() if (_reports == nullptr) goto out; + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + /* advertise sensor topic */ struct gyro_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6010a2ad7..a3409cbac 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,7 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) - +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" +#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -585,6 +586,20 @@ LSM303D::init() reset(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); + + if (mag_ret == OK) { + log("default mag device"); + } + /* advertise mag topic */ struct mag_report zero_mag_report; memset(&zero_mag_report, 0, sizeof(zero_mag_report)); @@ -1670,7 +1685,7 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", MAG_DEVICE_PATH), + CDev("LSM303D_mag", "/dev/lsm303d_mag"), _parent(parent) { } @@ -1736,7 +1751,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1747,7 +1762,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1755,7 +1770,7 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); /* don't fail if open cannot be opened */ if (0 <= fd_mag) { @@ -1790,10 +1805,10 @@ test() int ret; /* get the driver */ - fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd_accel < 0) - err(1, "%s open failed", ACCEL_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); @@ -1819,10 +1834,10 @@ test() struct mag_report m_report; /* get the driver */ - fd_mag = open(MAG_DEVICE_PATH, O_RDONLY); + fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd_mag < 0) - err(1, "%s open failed", MAG_DEVICE_PATH); + err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); /* check if mag is onboard or external */ if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6bfa583fb..ce0010752 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -75,6 +75,9 @@ #define DIR_READ 0x80 #define DIR_WRITE 0x00 +#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel" +#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro" + // MPU 6000 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 @@ -359,7 +362,7 @@ private: extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } MPU6000::MPU6000(int bus, spi_dev_e device) : - SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), + SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), @@ -468,6 +471,20 @@ MPU6000::init() /* fetch an initial set of measurements for advertisement */ measure(); + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default accel device"); + } + + /* try to claim the generic accel node as well - it's OK if we fail at this */ + ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); + + if (ret == OK) { + log("default gyro device"); + } + if (gyro_ret != OK) { _gyro_topic = -1; } else { @@ -1290,7 +1307,7 @@ MPU6000::print_info() } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : - CDev("MPU6000_gyro", GYRO_DEVICE_PATH), + CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent) { } @@ -1352,7 +1369,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) goto fail; @@ -1384,17 +1401,17 @@ test() ssize_t sz; /* get the driver */ - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)", - ACCEL_DEVICE_PATH); + MPU_DEVICE_PATH_ACCEL); /* get the driver */ - int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", MPU_DEVICE_PATH_GYRO); /* reset to manual polling */ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -1452,7 +1469,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From 1fb406ba094a091c6976f752c3b6db71fc895605 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Dec 2013 10:44:29 +0100 Subject: Add also default descriptor for alternate sensors --- src/drivers/lsm303d/lsm303d.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a3409cbac..94966a963 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -586,11 +586,21 @@ LSM303D::init() reset(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ + /* register the first instance as plain name, the 2nd as two and counting */ ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); if (ret == OK) { log("default accel device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* try to claim the generic accel node as well - it's OK if we fail at this */ @@ -598,6 +608,16 @@ LSM303D::init() if (mag_ret == OK) { log("default mag device"); + + } else { + + unsigned instance = 1; + do { + char name[32]; + sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); + ret = register_driver(name, &fops, 0666, (void *)this); + instance++; + } while (ret); } /* advertise mag topic */ -- cgit v1.2.3 From f24479c27a3627d315eee0f329da8aca8741eebe Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:39:07 +1100 Subject: lsm303d: dump I2C control registers in regdump --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 94966a963..c535c4e87 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1643,6 +1643,8 @@ LSM303D::print_registers() const char *name; } regmap[] = { { ADDR_WHO_AM_I, "WHO_AM_I" }, + { 0x02, "I2C_CONTROL1" }, + { 0x15, "I2C_CONTROL2" }, { ADDR_STATUS_A, "STATUS_A" }, { ADDR_STATUS_M, "STATUS_M" }, { ADDR_CTRL_REG0, "CTRL_REG0" }, -- cgit v1.2.3 From 6145e69fc62c7448baf0531d5c492e26b9225b01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:43:54 +1100 Subject: device: added register_class_devname() API this allows drivers to register generic device names for a device class, with automatic class instance handling --- src/drivers/device/cdev.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/drivers/device/device.h | 22 ++++++++++++++++++++++ 2 files changed, 58 insertions(+) (limited to 'src') diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 47ebcd40a..7954ce5ab 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -108,6 +108,42 @@ CDev::~CDev() unregister_driver(_devname); } +int +CDev::register_class_devname(const char *class_devname) +{ + if (class_devname == nullptr) { + return -EINVAL; + } + int class_instance = 0; + int ret = -ENOSPC; + while (class_instance < 4) { + if (class_instance == 0) { + ret = register_driver(class_devname, &fops, 0666, (void *)this); + if (ret == OK) break; + } else { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + ret = register_driver(name, &fops, 0666, (void *)this); + if (ret == OK) break; + } + class_instance++; + } + if (class_instance == 4) + return ret; + return class_instance; +} + +int +CDev::unregister_class_devname(const char *class_devname, unsigned class_instance) +{ + if (class_instance > 0) { + char name[32]; + snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + return unregister_driver(name); + } + return unregister_driver(class_devname); +} + int CDev::init() { diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index a9ed5d77c..0235f6284 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -396,6 +396,25 @@ protected: */ virtual int close_last(struct file *filp); + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @return class_instamce Class instance created, or -errno on failure + */ + virtual int register_class_devname(const char *class_devname); + + /** + * Register a class device name, automatically adding device + * class instance suffix if need be. + * + * @param class_devname Device class name + * @param class_instance Device class instance from register_class_devname() + * @return OK on success, -errno otherwise + */ + virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + private: static const unsigned _max_pollwaiters = 8; @@ -488,4 +507,7 @@ private: } // namespace device +// class instance for primary driver of each class +#define CLASS_DEVICE_PRIMARY 0 + #endif /* _DEVICE_DEVICE_H */ -- cgit v1.2.3 From c5097a6561928e9d5e88926e5be28df845256d91 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:44:58 +1100 Subject: hmc5883: use register_class_devname() --- src/drivers/hmc5883/hmc5883.cpp | 35 +++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22ad301ff..d3b99ae66 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -155,6 +155,7 @@ private: float _range_scale; float _range_ga; bool _collect_phase; + int _class_instance; orb_advert_t _mag_topic; @@ -322,6 +323,7 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _mag_topic(-1), + _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -352,6 +354,9 @@ HMC5883::~HMC5883() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _class_instance); + // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); @@ -375,13 +380,17 @@ HMC5883::init() /* reset the device configuration */ reset(); - /* get a publish handle on the mag topic */ - struct mag_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + _class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the mag topic if we are + * the primary mag */ + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); - if (_mag_topic < 0) - debug("failed to create sensor_mag object"); + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } ret = OK; /* sensor is ok, but not calibrated */ @@ -876,8 +885,10 @@ HMC5883::collect() } #endif - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + if (_mag_topic != -1) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } /* post a report to the ring */ if (_reports->force(&new_report)) { @@ -1292,7 +1303,7 @@ test() int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -1389,10 +1400,10 @@ int calibrate() { int ret; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) - err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); + err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH); if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1414,7 +1425,7 @@ int calibrate() void reset() { - int fd = open(MAG_DEVICE_PATH, O_RDONLY); + int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From 1d5f0a1433f838fc553461c808129a89d917058a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:10 +1100 Subject: l3gd20: use register_class_devname() --- src/drivers/l3gd20/l3gd20.cpp | 37 ++++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index a27bc5280..78a086b11 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -66,6 +66,8 @@ #include #include +#define L3GD20_DEVICE_PATH "/dev/l3gd20" + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -199,6 +201,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; + int _class_instance; unsigned _current_rate; unsigned _orientation; @@ -317,6 +320,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), + _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), @@ -346,6 +350,9 @@ L3GD20::~L3GD20() if (_reports != nullptr) delete _reports; + if (_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _class_instance); + /* delete the perf counter */ perf_free(_sample_perf); } @@ -365,17 +372,13 @@ L3GD20::init() if (_reports == nullptr) goto out; - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - /* advertise sensor topic */ - struct gyro_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + _class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise sensor topic */ + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); + } reset(); @@ -743,7 +746,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -922,7 +925,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new L3GD20(1 /* XXX magic number */, GYRO_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); + g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO); if (g_dev == nullptr) goto fail; @@ -931,7 +934,7 @@ start() goto fail; /* set the poll rate to default, starts automatic data collection */ - fd = open(GYRO_DEVICE_PATH, O_RDONLY); + fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) goto fail; @@ -963,10 +966,10 @@ test() ssize_t sz; /* get the driver */ - fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY); + fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd_gyro < 0) - err(1, "%s open failed", GYRO_DEVICE_PATH); + err(1, "%s open failed", L3GD20_DEVICE_PATH); /* reset to manual polling */ if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) @@ -999,7 +1002,7 @@ test() void reset() { - int fd = open(GYRO_DEVICE_PATH, O_RDONLY); + int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); if (fd < 0) err(1, "failed "); -- cgit v1.2.3 From 02e7f7fa85ec4d443e32cd320d80a4152712a22c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:28 +1100 Subject: lsm303d: use register_class_devname() --- src/drivers/lsm303d/lsm303d.cpp | 121 ++++++++++++++++++++-------------------- 1 file changed, 62 insertions(+), 59 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index c535c4e87..beac3c751 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -80,8 +80,8 @@ static const int ERROR = -1; #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) -#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" #define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel" +#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag" /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F @@ -277,7 +277,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_advert_t _mag_topic; + int _class_instance; unsigned _accel_read; unsigned _mag_read; @@ -467,6 +467,8 @@ public: 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 init(); + protected: friend class LSM303D; @@ -474,6 +476,9 @@ protected: private: LSM303D *_parent; + orb_advert_t _mag_topic; + int _mag_class_instance; + void measure(); void measure_trampoline(void *arg); @@ -495,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _mag_topic(-1), + _class_instance(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), @@ -546,6 +551,9 @@ LSM303D::~LSM303D() if (_mag_reports != nullptr) delete _mag_reports; + if (_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance); + delete _mag; /* delete the perf counter */ @@ -575,10 +583,6 @@ LSM303D::init() goto out; /* advertise accel topic */ - struct accel_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -586,53 +590,22 @@ LSM303D::init() reset(); - /* register the first instance as plain name, the 2nd as two and counting */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", ACCEL_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - mag_ret = register_driver(MAG_DEVICE_PATH, &fops, 0666, (void *)this); - - if (mag_ret == OK) { - log("default mag device"); - - } else { - - unsigned instance = 1; - do { - char name[32]; - sprintf(name, "%s%d", MAG_DEVICE_PATH, instance); - ret = register_driver(name, &fops, 0666, (void *)this); - instance++; - } while (ret); + /* do CDev init for the mag device node */ + ret = _mag->init(); + if (ret != OK) { + warnx("MAG init failed"); + goto out; } - /* advertise mag topic */ - struct mag_report zero_mag_report; - memset(&zero_mag_report, 0, sizeof(zero_mag_report)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); - - /* do CDev init for the mag device node, keep it optional */ - mag_ret = _mag->init(); - - if (mag_ret != OK) { - _mag_topic = -1; + _class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary accel device, so advertise to + // the ORB + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); } - ret = OK; out: return ret; } @@ -1544,8 +1517,10 @@ LSM303D::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + if (_accel_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } _accel_read++; @@ -1616,8 +1591,10 @@ LSM303D::mag_measure() /* notify anyone waiting for data */ poll_notify(POLLIN); - /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); + if (_mag->_mag_topic != -1) { + /* publish for subscribers */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } _mag_read++; @@ -1707,13 +1684,39 @@ LSM303D::toggle_logging() } LSM303D_mag::LSM303D_mag(LSM303D *parent) : - CDev("LSM303D_mag", "/dev/lsm303d_mag"), - _parent(parent) + CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(-1), + _mag_class_instance(-1) { } LSM303D_mag::~LSM303D_mag() { + if (_mag_class_instance != -1) + unregister_class_devname(MAG_DEVICE_PATH, _mag_class_instance); +} + +int +LSM303D_mag::init() +{ + int ret; + + ret = CDev::init(); + if (ret != OK) + goto out; + + _mag_class_instance = register_class_devname(MAG_DEVICE_PATH); + if (_mag_class_instance == CLASS_DEVICE_PRIMARY) { + // we are the primary mag device, so advertise to + // the ORB + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); + } + +out: + return ret; } void @@ -1773,7 +1776,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* XXX magic number */, LSM303D_DEVICE_PATH_MAG, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); @@ -1892,7 +1895,7 @@ test() void reset() { - int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); if (fd < 0) err(1, "failed "); @@ -1903,7 +1906,7 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); - fd = open(MAG_DEVICE_PATH, O_RDONLY); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); -- cgit v1.2.3 From 1fc122562c16dfd419e832b95292678a7db8ec22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Dec 2013 22:45:39 +1100 Subject: mpu6000: use register_class_devname() --- src/drivers/mpu6000/mpu6000.cpp | 114 +++++++++++++++++++++++++--------------- 1 file changed, 71 insertions(+), 43 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ce0010752..6145a5117 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -211,16 +211,17 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; + int _accel_class_instance; RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - orb_advert_t _gyro_topic; - unsigned _reads; unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; perf_counter_t _sample_perf; math::LowPassFilter2p _accel_filter_x; @@ -349,12 +350,17 @@ public: 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 init(); + protected: friend class MPU6000; void parent_poll_notify(); + private: MPU6000 *_parent; + orb_advert_t _gyro_topic; + int _gyro_class_instance; }; @@ -370,12 +376,13 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _gyro_topic(-1), - _reads(0), _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -420,8 +427,13 @@ MPU6000::~MPU6000() if (_gyro_reports != nullptr) delete _gyro_reports; + if (_accel_class_instance != -1) + unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance); + /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); } int @@ -466,38 +478,23 @@ MPU6000::init() _gyro_scale.z_scale = 1.0f; /* do CDev init for the gyro device node, keep it optional */ - gyro_ret = _gyro->init(); + ret = _gyro->init(); + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } /* fetch an initial set of measurements for advertisement */ measure(); - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(ACCEL_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default accel device"); - } - - /* try to claim the generic accel node as well - it's OK if we fail at this */ - ret = register_driver(GYRO_DEVICE_PATH, &fops, 0666, (void *)this); - - if (ret == OK) { - log("default gyro device"); - } - - if (gyro_ret != OK) { - _gyro_topic = -1; - } else { - gyro_report gr; - _gyro_reports->get(&gr); - - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); - } - - /* advertise accel topic */ - accel_report ar; - _accel_reports->get(&ar); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); + if (_accel_class_instance == CLASS_DEVICE_PRIMARY) { + /* advertise accel topic */ + accel_report ar; + _accel_reports->get(&ar); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); + } out: return ret; @@ -677,6 +674,8 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) if (_accel_reports->empty()) return -EAGAIN; + perf_count(_accel_reads); + /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; @@ -694,12 +693,12 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) int MPU6000::self_test() { - if (_reads == 0) { + if (perf_event_count(_sample_perf) == 0) { measure(); } /* return 0 on success, 1 else */ - return (_reads > 0) ? 0 : 1; + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; } int @@ -771,6 +770,8 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) if (_gyro_reports->empty()) return -EAGAIN; + perf_count(_gyro_reads); + /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; @@ -1180,9 +1181,6 @@ MPU6000::measure() if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) return; - /* count measurement */ - _reads++; - /* * Convert from big to little endian */ @@ -1287,10 +1285,11 @@ MPU6000::measure() poll_notify(POLLIN); _gyro->parent_poll_notify(); - /* and publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); - if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + if (_accel_topic != -1) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + if (_gyro->_gyro_topic != -1) { + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1301,19 +1300,48 @@ void MPU6000::print_info() { perf_print_counter(_sample_perf); - printf("reads: %u\n", _reads); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), - _parent(parent) + _parent(parent), + _gyro_class_instance(-1) { } MPU6000_gyro::~MPU6000_gyro() { + if (_gyro_class_instance != -1) + unregister_class_devname(GYRO_DEVICE_PATH, _gyro_class_instance); +} + +int +MPU6000_gyro::init() +{ + int ret; + + // do base class init + ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + debug("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH); + if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) { + gyro_report gr; + memset(&gr, 0, sizeof(gr)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); + } + +out: + return ret; } void -- cgit v1.2.3 From 09ece4306e929b9ffbd7d21a50c5d3d21265bd87 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:31 +1100 Subject: l3gd20: close fds before exit --- src/drivers/l3gd20/l3gd20.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 78a086b11..910131c6a 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -942,6 +942,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -990,6 +992,8 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + close(fd_gyro); + /* XXX add poll-rate tests here too */ reset(); @@ -1013,6 +1017,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + exit(0); } -- cgit v1.2.3 From acd0a70dca1066e09fc98ed6576682b6de330e7c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:43 +1100 Subject: lsm303d: close fds before exit --- src/drivers/lsm303d/lsm303d.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index beac3c751..11e5b95a7 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1804,6 +1804,8 @@ start() } } + close(fd); + close(fd_mag); exit(0); fail: @@ -1885,6 +1887,9 @@ test() /* XXX add poll-rate tests here too */ + close(fd_accel); + close(fd_mag); + reset(); errx(0, "PASS"); } @@ -1906,6 +1911,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "accel pollrate reset failed"); + close(fd); + fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { @@ -1916,6 +1923,8 @@ reset() err(1, "mag pollrate reset failed"); } + close(fd); + exit(0); } -- cgit v1.2.3 From f0d84d4826a6563693ae0abd6ac1f72a3eafdc68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 8 Dec 2013 20:46:54 +1100 Subject: mpu6000: close fds before exit --- src/drivers/mpu6000/mpu6000.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6145a5117..ff921274b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1405,6 +1405,8 @@ start() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) goto fail; + close(fd); + exit(0); fail: @@ -1508,6 +1510,8 @@ reset() if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) err(1, "driver poll restart failed"); + close(fd); + exit(0); } -- cgit v1.2.3 From 96881d88106c421a4ea44118b754f90d47b94e4f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Dec 2013 10:03:50 +1100 Subject: ms5611: check for all zero in the prom when SPI CLK fails we get all zero data --- src/drivers/ms5611/ms5611_spi.cpp | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index e9dff5a8b..bc4074c55 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -236,9 +236,12 @@ MS5611_SPI::_read_prom() usleep(3000); /* read and convert PROM words */ + bool all_zero = true; for (int i = 0; i < 8; i++) { uint8_t cmd = (ADDR_PROM_SETUP + (i * 2)); _prom.c[i] = _reg16(cmd); + if (_prom.c[i] != 0) + all_zero = false; //debug("prom[%u]=0x%x", (unsigned)i, (unsigned)_prom.c[i]); } @@ -247,6 +250,10 @@ MS5611_SPI::_read_prom() if (ret != OK) { debug("crc failed"); } + if (all_zero) { + debug("prom all zero"); + ret = -EIO; + } return ret; } -- cgit v1.2.3 From 91870953d951bc0e15640363bcc1d826cb5ed1b1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 09:13:38 +1100 Subject: mpu6000: treat all zero data from mpu6k as bad --- src/drivers/mpu6000/mpu6000.cpp | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index ff921274b..c95d11c83 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -223,6 +223,7 @@ private: perf_counter_t _accel_reads; perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -384,6 +385,7 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _accel_reads(perf_alloc(PC_COUNT, "mpu6000_accel_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -434,6 +436,7 @@ MPU6000::~MPU6000() perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); + perf_free(_bad_transfers); } int @@ -1013,9 +1016,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) uint8_t MPU6000::read_reg(unsigned reg) { - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1028,9 +1029,7 @@ MPU6000::read_reg(unsigned reg) uint16_t MPU6000::read_reg16(unsigned reg) { - uint8_t cmd[3]; - - cmd[0] = reg | DIR_READ; + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; // general register transfer at low clock speed set_frequency(MPU6000_LOW_BUS_SPEED); @@ -1195,6 +1194,20 @@ MPU6000::measure() report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + return; + } + + /* * Swap axes and negate y */ -- cgit v1.2.3 From d43e3394b07b5999dff5d04a3dd77f96d76c1134 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 14:03:16 +1100 Subject: l3gd20: added rescheduling and error checking --- src/drivers/l3gd20/l3gd20.cpp | 29 ++++++++++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 910131c6a..1077eb43b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -209,6 +209,8 @@ private: unsigned _read; perf_counter_t _sample_perf; + perf_counter_t _reschedules; + perf_counter_t _errors; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -325,6 +327,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : _orientation(SENSOR_BOARD_ROTATION_270_DEG), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), + _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), + _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ) @@ -355,6 +359,8 @@ L3GD20::~L3GD20() /* delete the perf counter */ perf_free(_sample_perf); + perf_free(_reschedules); + perf_free(_errors); } int @@ -746,7 +752,7 @@ L3GD20::reset() /* set default configuration */ write_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ - write_reg(ADDR_CTRL_REG3, 0); /* no interrupts - we don't use them */ + write_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ write_reg(ADDR_CTRL_REG4, REG4_BDU); write_reg(ADDR_CTRL_REG5, 0); @@ -776,6 +782,17 @@ L3GD20::measure_trampoline(void *arg) void L3GD20::measure() { +#ifdef GPIO_EXTI_GYRO_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + return; + } +#endif + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -798,6 +815,16 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); +#ifdef GPIO_EXTI_GYRO_DRDY + if (raw_report.status & 0xF != 0xF) { + /* + we waited for DRDY, but did not see DRDY on all axes + when we captured. That means a transfer error of some sort + */ + perf_count(_errors); + return; + } +#endif /* * 1) Scale raw value to SI units using scaling from datasheet. * 2) Subtract static offset (in SI units) -- cgit v1.2.3 From cf78440ee6cb8c1469f53ca096d7388524facd59 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 10 Dec 2013 15:09:36 +1100 Subject: drv_hrt: added note on why an uninitialised hrt_call is safe --- src/drivers/stm32/drv_hrt.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index dc28f446b..1bd251bc2 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -720,7 +720,6 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_call_init(entry); hrt_call_internal(entry, hrt_absolute_time() + delay, interval, @@ -734,6 +733,13 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte irqstate_t flags = irqsave(); /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ if (entry->deadline != 0) sq_rem(&entry->link, &callout_queue); -- cgit v1.2.3 From 17502cbde43cf96c55af1811723ce84ca2a1fc27 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 09:02:31 +1100 Subject: l3gd20: fixed a warning --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1077eb43b..1d437df2b 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -816,7 +816,7 @@ L3GD20::measure() transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); #ifdef GPIO_EXTI_GYRO_DRDY - if (raw_report.status & 0xF != 0xF) { + if ((raw_report.status & 0xF) != 0xF) { /* we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort -- cgit v1.2.3 From e808e015dd84c234f9689daf90aedf0162d7d2f2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 14:14:03 +1100 Subject: LowPassFilter: allow for filtering to be disabled using bandwidth of 0 gives no filtering --- src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src') diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index efb17225d..3699d9bce 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -46,6 +46,10 @@ namespace math void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) { _cutoff_freq = cutoff_freq; + if (_cutoff_freq <= 0.0f) { + // no filtering + return; + } float fr = sample_freq/_cutoff_freq; float ohm = tanf(M_PI_F/fr); float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; @@ -58,6 +62,10 @@ void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) float LowPassFilter2p::apply(float sample) { + if (_cutoff_freq <= 0.0f) { + // no filtering + return sample; + } // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { -- cgit v1.2.3 From 8f90efa312b4bccbacb9e9173e2cba7d7b4bc193 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Dec 2013 14:14:33 +1100 Subject: l3gd20: print more perf counters and make DRDY usage clearer --- src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 1d437df2b..d639acba1 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -779,10 +779,16 @@ L3GD20::measure_trampoline(void *arg) dev->measure(); } +#ifdef GPIO_EXTI_GYRO_DRDY +# define L3GD20_USE_DRDY 1 +#else +# define L3GD20_USE_DRDY 0 +#endif + void L3GD20::measure() { -#ifdef GPIO_EXTI_GYRO_DRDY +#if L3GD20_USE_DRDY // if the gyro doesn't have any data ready then re-schedule // for 100 microseconds later. This ensures we don't double // read a value and then miss the next value @@ -815,7 +821,7 @@ L3GD20::measure() raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report)); -#ifdef GPIO_EXTI_GYRO_DRDY +#if L3GD20_USE_DRDY if ((raw_report.status & 0xF) != 0xF) { /* we waited for DRDY, but did not see DRDY on all axes @@ -902,6 +908,8 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); + perf_print_counter(_reschedules); + perf_print_counter(_errors); _reports->print_info("report queue"); } -- cgit v1.2.3 From 6016fbe55d3fd402012168b5a704a550bcfc4d28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 11:30:07 +1100 Subject: Merged PX4IO crc checks and force update --- src/drivers/px4io/px4io.cpp | 46 ++++++++++++++++++++++++++++++++++- src/drivers/px4io/px4io_uploader.cpp | 5 +++- src/modules/px4iofirmware/protocol.h | 3 +++ src/modules/px4iofirmware/registers.c | 26 ++++++++++++++++++++ 4 files changed, 78 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ef6ca04e9..f5fb618c3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -95,6 +95,7 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -2146,6 +2147,16 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); break; + case PX4IO_REBOOT_BOOTLOADER: + if (system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + return -EINVAL; + + /* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg); + // we don't expect a reply from this operation + ret = OK; + break; + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2673,6 +2684,39 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + printf("usage: px4io forceupdate MAGIC filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || @@ -2690,5 +2734,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index d01dedb0d..41f93a8ee 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -274,7 +274,10 @@ PX4IO_Uploader::drain() int ret; do { - ret = recv(c, 1000); + // the small recv timeout here is to allow for fast + // drain when rebooting the io board for a forced + // update of the fw without using the safety switch + ret = recv(c, 40); #ifdef UDEBUG if (ret == OK) { diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 5e5396782..2856bdea4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -186,6 +186,9 @@ enum { /* DSM bind states */ /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 86a40bc22..6785e5366 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -45,6 +45,8 @@ #include #include +#include +#include #include "px4io.h" #include "protocol.h" @@ -154,6 +156,7 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_VBATT_SCALE] = 10000, #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, + [PX4IO_P_SETUP_REBOOT_BL] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -501,6 +504,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_REBOOT_BL: + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // don't allow reboot while armed + break; + } + + // check the magic value + if (value != PX4IO_REBOOT_BL_MAGIC) + break; + + // note that we don't set BL_WAIT_MAGIC in + // BKP_DR1 as that is not necessary given the + // timing of the forceupdate command. The + // bootloader on px4io waits for enough time + // anyway, and this method works with older + // bootloader versions (tested with both + // revision 3 and revision 4). + + up_systemreset(); + break; + case PX4IO_P_SETUP_DSM: dsm_bind(value & 0x0f, (value >> 4) & 7); break; -- cgit v1.2.3 From 5b7d1af5d83554d3119e3e0669d6429fc1aef262 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 13 Dec 2013 12:44:11 +1100 Subject: Merged crccheck command --- src/drivers/px4io/px4io.cpp | 64 +++++++++++++++++++++++++++++++++-- src/modules/px4iofirmware/protocol.h | 2 ++ src/modules/px4iofirmware/px4io.c | 20 +++++++++++ src/modules/px4iofirmware/registers.c | 1 + 4 files changed, 85 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f5fb618c3..cc3815f3e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include @@ -96,6 +97,7 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) #define PX4IO_REBOOT_BOOTLOADER _IOC(0xff00, 2) +#define PX4IO_CHECK_CRC _IOC(0xff00, 3) #define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz #define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz @@ -1660,11 +1662,13 @@ void PX4IO::print_status() { /* basic configuration */ - printf("protocol %u hardware %u bootloader %u buffer %uB\n", + printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC+1)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), @@ -2157,6 +2161,19 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = OK; break; + case PX4IO_CHECK_CRC: { + /* check IO firmware CRC against passed value */ + uint32_t io_crc = 0; + ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2); + if (ret != OK) + return ret; + if (io_crc != arg) { + debug("crc mismatch 0x%08x 0x%08x", (unsigned)io_crc, arg); + return -EINVAL; + } + break; + } + case PX4IO_INAIR_RESTART_ENABLE: /* set/clear the 'in-air restart' bit */ @@ -2717,6 +2734,49 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "checkcrc")) { + /* + check IO CRC against CRC of a file + */ + if (argc <= 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + if (g_dev == nullptr) { + printf("px4io is not started\n"); + exit(1); + } + int fd = open(argv[2], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[2], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); + } + if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 2856bdea4..cffabbb45 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -189,6 +189,8 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index cd9bd197b..745bd5705 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -124,6 +125,22 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } + +static void +calculate_fw_crc(void) +{ +#define APP_SIZE_MAX 0xf000 +#define APP_LOAD_ADDRESS 0x08001000 + // compute CRC of the current firmware + uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { + uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); + sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); + } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; + r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; +} + int user_start(int argc, char *argv[]) { @@ -136,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the high-resolution time/callout interface */ hrt_init(); + /* calculate our fw CRC so FMU can decide if we need to update */ + calculate_fw_crc(); + /* * Poll at 1ms intervals for received bytes that have not triggered * a DMA event. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 6785e5366..1a8519aec 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -157,6 +157,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, + [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) -- cgit v1.2.3 From 3a40ea8338206b247aadb40e3709a22e0699135b Mon Sep 17 00:00:00 2001 From: Holger Steinhaus L Date: Mon, 11 Nov 2013 12:14:03 +0100 Subject: more precise range conversion for SBus input signals --- src/modules/px4iofirmware/sbus.c | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index c523df6ca..8034e7077 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -55,6 +55,24 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +/* + Measured values with Futaba FX-30/R6108SB: + -+100% on TX: PCM 1.100/1.520/1.950ms -> SBus raw values: 350/1024/1700 (100% ATV) + -+140% on TX: PCM 0.930/1.520/2.112ms -> SBus raw values: 78/1024/1964 (140% ATV) + -+152% on TX: PCM 0.884/1.520/2.160ms -> SBus raw values: 1/1024/2047 (140% ATV plus dirty tricks) +*/ + +/* define range mapping here, -+100% -> 1000..2000 */ +#define SBUS_RANGE_MIN 350.0f +#define SBUS_RANGE_MAX 1700.0f + +#define SBUS_TARGET_MIN 1000.0f +#define SBUS_TARGET_MAX 2000.0f + +/* pre-calculate the floating point stuff as far as possible at compile time */ +#define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN)) +#define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f)) + static int sbus_fd = -1; static hrt_abstime last_rx_time; @@ -234,8 +252,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint } } - /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ - values[channel] = (value / 2) + 998; + + /* convert 0-2048 values to 1000-2000 ppm encoding in a not too sloppy fashion */ + values[channel] = (uint16_t)(value * SBUS_SCALE_FACTOR +.5f) + SBUS_SCALE_OFFSET; } /* decode switch channels if data fields are wide enough */ -- cgit v1.2.3 From a673fa3926d18163841f817c08bf089a0a8224f7 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus L Date: Sun, 17 Nov 2013 12:56:26 +0100 Subject: Non-destructive handling of failsafe signals, distinction between frame loss and signal loss. This kind of handling might be moved upstream into the application, once alarms are propagated by the ORB system. This change is compatible with RX failsafe settings, but does not rely on it (uses SBus flags instead). --- src/modules/px4iofirmware/sbus.c | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 8034e7077..68af85dc9 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -54,6 +54,9 @@ #define SBUS_FRAME_SIZE 25 #define SBUS_INPUT_CHANNELS 16 +#define SBUS_FLAGS_BYTE 23 +#define SBUS_FAILSAFE_BIT 3 +#define SBUS_FRAMELOST_BIT 2 /* Measured values with Futaba FX-30/R6108SB: @@ -220,15 +223,6 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint return false; } - /* if the failsafe or connection lost bit is set, we consider the frame invalid */ - if ((frame[23] & (1 << 2)) && /* signal lost */ - (frame[23] & (1 << 3))) { /* failsafe */ - - /* actively announce signal loss */ - *values = 0; - return false; - } - /* we have received something we think is a frame */ last_frame_time = frame_time; @@ -262,13 +256,28 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint chancount = 18; /* channel 17 (index 16) */ - values[16] = (frame[23] & (1 << 0)) * 1000 + 998; + values[16] = (frame[SBUS_FLAGS_BYTE] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ - values[17] = (frame[23] & (1 << 1)) * 1000 + 998; + values[17] = (frame[SBUS_FLAGS_BYTE] & (1 << 1)) * 1000 + 998; } /* note the number of channels decoded */ *num_values = chancount; + /* decode and handle failsafe and frame-lost flags */ + if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ + /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ + values[2] = 900; + } + else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ + /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + * + * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this + * condition as fail-safe greatly reduces the reliability and range of the radio link, + * e.g. by prematurely issueing return-to-launch!!! */ + + // values[2] = 888; // marker for debug purposes + } + return true; } -- cgit v1.2.3 From 9883a346a022131b20a292d96a87f20fb7921b1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Dec 2013 18:01:55 +0100 Subject: First stab at implementing better RSSI based connection status estimation, still needs some work and testing --- src/drivers/drv_rc_input.h | 3 +++ src/modules/px4iofirmware/controls.c | 21 +++++++++++++++++---- src/modules/px4iofirmware/protocol.h | 1 + src/modules/px4iofirmware/px4io.h | 2 +- src/modules/px4iofirmware/registers.c | 2 +- src/modules/px4iofirmware/sbus.c | 17 ++++++++++------- 6 files changed, 33 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 78ffad9d7..86e5a149a 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,6 +89,9 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + int32_t rssi; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 5c621cfb2..194d8aab9 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -94,6 +94,9 @@ controls_tick() { * other. Don't do that. */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + uint16_t rssi = 0; + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); @@ -104,14 +107,15 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + + rssi = 1000; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - r_raw_rc_count = 8; } perf_end(c_gather_sbus); @@ -122,10 +126,19 @@ controls_tick() { */ perf_begin(c_gather_ppm); bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) + if (ppm_updated) { + + /* XXX sample RSSI properly here */ + rssi = 1000; + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + } perf_end(c_gather_ppm); + /* limit number of channels to allowable data size */ + if (r_raw_rc_count > PX4IO_INPUT_CHANNELS) + r_raw_rc_count = PX4IO_INPUT_CHANNELS; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -221,7 +234,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0) { + if (assigned_channels == 0 || rssi == 0) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index cffabbb45..11e380397 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -124,6 +124,7 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 4fea0288c..b9c9e0232 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -209,7 +209,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1a8519aec..3f9e111ba 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -114,7 +114,7 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + 24)] = 0 // XXX ensure we have enough space to decode beefy RX, will be replaced by patch soon }; /** diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 68af85dc9..2153fadc8 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, max_channels); + return sbus_decode(now, values, num_values, rssi, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -266,8 +266,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ - /* emulate throttle failsafe for maximum compatibility and do not destroy any channel data */ - values[2] = 900; + /* report that we failed to read anything valid off the receiver */ + *rssi = 0; + return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented @@ -276,8 +277,10 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - // values[2] = 888; // marker for debug purposes + *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } + *rssi = 1000; + return true; } -- cgit v1.2.3 From 00dc339d2ece42161e36af7cc52c042c4fcec697 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 14:52:57 +0100 Subject: Improved S.Bus scaling based on scope measurements --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 2153fadc8..65e14a18f 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,8 +66,8 @@ */ /* define range mapping here, -+100% -> 1000..2000 */ -#define SBUS_RANGE_MIN 350.0f -#define SBUS_RANGE_MAX 1700.0f +#define SBUS_RANGE_MIN 215.0f +#define SBUS_RANGE_MAX 1790.0f #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f -- cgit v1.2.3 From 10b2dc67e4529759b9dcbef6a18360eba610b5a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 14:54:02 +0100 Subject: Allow forceupdate in all conditions --- src/drivers/px4io/px4io.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5d35ecb8d..685a9746d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2653,22 +2653,22 @@ px4io_main(int argc, char *argv[]) the user to hold the safety switch down */ if (argc <= 3) { - printf("usage: px4io forceupdate MAGIC filename\n"); + warnx("usage: px4io forceupdate MAGIC filename"); exit(1); } if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } - // tear down the px4io instance - delete g_dev; + // tear down the px4io instance + delete g_dev; + } // upload the specified firmware const char *fn[2]; -- cgit v1.2.3 From ea10d55d71b25b4440c6b4dc139dda519a0d2797 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 15:08:56 +0100 Subject: Auto-update of IO firmware, shorter preflight check alarm --- ROMFS/px4fmu_common/init.d/rcS | 47 ++++++++++++++++++------ src/drivers/px4io/px4io.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 38 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f1df99048..f551c1aa8 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,6 +8,8 @@ # set MODE autostart +set logfile /fs/microsd/bootlog.txt + # # Try to mount the microSD card. # @@ -147,26 +149,49 @@ then nshterm /dev/ttyACM0 & fi - # # Upgrade PX4IO firmware # - if px4io detect + + if [ -f /etc/extras/px4io-v2_default.bin ] + then + set io_file /etc/extras/px4io-v2_default.bin + else + set io_file /etc/extras/px4io-v1_default.bin + fi + + if px4io start then - echo "PX4IO running, not upgrading" + echo "PX4IO OK" + echo "PX4IO OK" >> $logfile + fi + + if px4io checkcrc $io_file + then + echo "PX4IO CRC OK" + echo "PX4IO CRC OK" >> $logfile else - echo "Attempting to upgrade PX4IO" - if px4io update + echo "PX4IO CRC failure" + echo "PX4IO CRC failure" >> $logfile + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file then - if [ -d /fs/microsd ] + usleep 200000 + if px4io start then - echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log + echo "PX4IO restart OK" + echo "PX4IO restart OK" >> $logfile + tone_alarm MSPAA + else + echo "PX4IO restart failed" + echo "PX4IO restart failed" >> $logfile + tone_alarm MNGGG + sh /etc/init.d/rc.error fi - - # Allow IO to safely kick back to app - usleep 200000 else - echo "No PX4IO to upgrade here" + echo "PX4IO update failed" + echo "PX4IO update failed" >> $logfile + tone_alarm MNGGG fi fi diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 685a9746d..569f1e413 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2229,7 +2229,7 @@ void start(int argc, char *argv[]) { if (g_dev != nullptr) - errx(1, "already loaded"); + errx(0, "already loaded"); /* allocate the interface */ device::Device *interface = get_interface(); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 07581459b..982b03782 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -200,7 +200,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 50; i++) + for (int i = 0; i < 14; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); -- cgit v1.2.3 From d6a6d59d2de7270fd978fcb6029edc3f315899a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Dec 2013 15:09:20 +0100 Subject: Further improved S.Bus scaling --- src/modules/px4iofirmware/sbus.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 65e14a18f..388502b40 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,8 +66,8 @@ */ /* define range mapping here, -+100% -> 1000..2000 */ -#define SBUS_RANGE_MIN 215.0f -#define SBUS_RANGE_MAX 1790.0f +#define SBUS_RANGE_MIN 200.0f +#define SBUS_RANGE_MAX 1800.0f #define SBUS_TARGET_MIN 1000.0f #define SBUS_TARGET_MAX 2000.0f -- cgit v1.2.3 From f4ac204f4650dedcde52aa9ec001fafb5e7c5284 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Dec 2013 18:32:46 +0100 Subject: Cranking up bus speeds for all sensors to achievable 10.4 MHz, will cut the bus lock time to half --- src/drivers/l3gd20/l3gd20.cpp | 2 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/mpu6000/mpu6000.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index d639acba1..670e51b97 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -316,7 +316,7 @@ private: }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call_interval(0), _reports(nullptr), _gyro_range_scale(0.0f), diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 11e5b95a7..969b5e25f 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -486,7 +486,7 @@ private: LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c95d11c83..be4e422b0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -170,7 +170,7 @@ SPI speed */ #define MPU6000_LOW_BUS_SPEED 1000*1000 -#define MPU6000_HIGH_BUS_SPEED 10*1000*1000 +#define MPU6000_HIGH_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */ class MPU6000_gyro; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index bc4074c55..26216e840 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -121,7 +121,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf) } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } -- cgit v1.2.3 From d53b00283e4b57611cc7e8c85a4dd1f18629aab0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 16 Dec 2013 12:02:01 +0100 Subject: PX4IO upgrade improvement --- src/drivers/px4io/px4io.cpp | 66 ++++++++++++++++++++++----------------------- 1 file changed, 33 insertions(+), 33 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 569f1e413..db882e623 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2562,6 +2562,39 @@ px4io_main(int argc, char *argv[]) if_test((argc > 2) ? strtol(argv[2], NULL, 0) : 0); } + if (!strcmp(argv[1], "forceupdate")) { + /* + force update of the IO firmware without requiring + the user to hold the safety switch down + */ + if (argc <= 3) { + warnx("usage: px4io forceupdate MAGIC filename"); + exit(1); + } + if (g_dev == nullptr) { + warnx("px4io is not started, still attempting upgrade"); + } else { + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + } + + // upload the specified firmware + const char *fn[2]; + fn[0] = argv[3]; + fn[1] = nullptr; + PX4IO_Uploader *up = new PX4IO_Uploader; + up->upload(&fn[0]); + delete up; + exit(0); + } + /* commands below here require a started driver */ if (g_dev == nullptr) @@ -2647,39 +2680,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "forceupdate")) { - /* - force update of the IO firmware without requiring - the user to hold the safety switch down - */ - if (argc <= 3) { - warnx("usage: px4io forceupdate MAGIC filename"); - exit(1); - } - if (g_dev == nullptr) { - warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } - - // tear down the px4io instance - delete g_dev; - } - - // upload the specified firmware - const char *fn[2]; - fn[0] = argv[3]; - fn[1] = nullptr; - PX4IO_Uploader *up = new PX4IO_Uploader; - up->upload(&fn[0]); - delete up; - exit(0); - } - if (!strcmp(argv[1], "checkcrc")) { /* check IO CRC against CRC of a file -- cgit v1.2.3 From bccf65cc28edb8e68b38765c895e9f74604df92e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 16 Dec 2013 22:16:51 +1100 Subject: mpu6000: disable interrupts during initial reset this seems to avoid a problem where the mpu6000 doesn't startup correctly if other devices are transferring at the same time. --- src/drivers/mpu6000/mpu6000.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index be4e422b0..bbc595af4 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -505,17 +505,26 @@ out: void MPU6000::reset() { + // if the mpu6000 is initialised after the l3gd20 and lsm303d + // then if we don't do an irqsave/irqrestore here the mpu6000 + // frequenctly comes up in a bad state where all transfers + // come as zero + irqstate_t state; + state = irqsave(); - // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); - // Wake up device and select GyroZ clock (better performance) + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); // Disable I2C bus (recommended on datasheet) write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); + up_udelay(1000); // SAMPLE RATE -- cgit v1.2.3 From 6dce57170e3ceaa3316446086f8a0cd12cc5e90c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Dec 2013 17:12:46 +0100 Subject: Hotfix: Fixed mapping of override channel --- src/drivers/px4io/px4io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index db882e623..e898b3e60 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1024,7 +1024,12 @@ PX4IO::io_set_rc_config() if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; - ichan = 4; + param_get(param_find("RC_MAP_MODE_SW"), &ichan); + + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) + input_map[ichan - 1] = 4; + + ichan = 5; for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) -- cgit v1.2.3 From 8c518aa23710ba0b9ad0c7ad2c03428ce8ddb290 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 14:25:35 +0100 Subject: Useful bits for high-rate logging --- ROMFS/px4fmu_common/init.d/rcS | 0 ROMFS/px4fmu_logging/init.d/rcS | 88 +++++++++++++++++++ ROMFS/px4fmu_test/init.d/rcS | 8 ++ makefiles/config_px4fmu-v2_logging.mk | 157 ++++++++++++++++++++++++++++++++++ src/systemcmds/tests/test_sensors.c | 67 +++++++++------ 5 files changed, 294 insertions(+), 26 deletions(-) mode change 100755 => 100644 ROMFS/px4fmu_common/init.d/rcS create mode 100644 ROMFS/px4fmu_logging/init.d/rcS mode change 100755 => 100644 ROMFS/px4fmu_test/init.d/rcS create mode 100644 makefiles/config_px4fmu-v2_logging.mk (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS old mode 100755 new mode 100644 diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS new file mode 100644 index 000000000..7b8856719 --- /dev/null +++ b/ROMFS/px4fmu_logging/init.d/rcS @@ -0,0 +1,88 @@ +#!nsh +# +# PX4FMU startup script for logging purposes +# + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +uorb start + +# +# Start sensor drivers here. +# + +ms5611 start +adc start + +# mag might be external +if hmc5883 start +then + echo "using HMC5883" +fi + +if mpu6000 start +then + echo "using MPU6000" +fi + +if l3gd20 start +then + echo "using L3GD20(H)" +fi + +if lsm303d start +then + set BOARD fmuv2 +else + set BOARD fmuv1 +fi + +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi +fi + +# +# Start the sensor collection task. +# IMPORTANT: this also loads param offsets +# ALWAYS start this task before the +# preflight_check. +# +if sensors start +then + echo "SENSORS STARTED" +fi + +sdlog2 start -r 250 -e -b 16 + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi \ No newline at end of file diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS old mode 100755 new mode 100644 index 7f161b053..6aa1d3d46 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,3 +2,11 @@ # # PX4FMU startup script for test hackery. # + +if sercon +then + echo "[init] USB interface connected" + + # Try to get an USB console + nshterm /dev/ttyACM0 & +fi \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_logging.mk b/makefiles/config_px4fmu-v2_logging.mk new file mode 100644 index 000000000..ed90f6464 --- /dev/null +++ b/makefiles/config_px4fmu-v2_logging.mk @@ -0,0 +1,157 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS, copy the px4iov2 firmware into +# the ROMFS if it's available +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_logging +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/px4io +MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/rgbled +MODULES += drivers/mpu6000 +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/hmc5883 +MODULES += drivers/ms5611 +MODULES += drivers/mb12xx +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors +MODULES += drivers/blinkm +MODULES += drivers/roboclaw +MODULES += drivers/airspeed +MODULES += drivers/ets_airspeed +MODULES += drivers/meas_airspeed +MODULES += modules/sensors + +# Needs to be burned to the ground and re-written; for now, +# just don't build it. +#MODULES += drivers/mkblctrl + +# +# System commands +# +MODULES += systemcmds/ramtron +MODULES += systemcmds/bl_update +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink +MODULES += modules/mavlink_onboard + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav +MODULES += examples/flow_position_estimator + +# +# Vehicle Control +# +#MODULES += modules/segway # XXX Needs GCC 4.7 fix +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/multirotor_att_control +MODULES += modules/multirotor_pos_control + +# +# Logging +# +MODULES += modules/sdlog2 + +# +# Unit tests +# +#MODULES += modules/unit_test +#MODULES += modules/commander/commander_tests + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index f6415b72f..096106ff3 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -78,7 +78,8 @@ static int accel(int argc, char *argv[]); static int gyro(int argc, char *argv[]); static int mag(int argc, char *argv[]); static int baro(int argc, char *argv[]); -static int mpu6k(int argc, char *argv[]); +static int accel1(int argc, char *argv[]); +static int gyro1(int argc, char *argv[]); /**************************************************************************** * Private Data @@ -93,7 +94,8 @@ struct { {"gyro", "/dev/gyro", gyro}, {"mag", "/dev/mag", mag}, {"baro", "/dev/baro", baro}, - {"mpu6k", "/dev/mpu6k", mpu6k}, + {"accel1", "/dev/accel1", accel1}, + {"gyro1", "/dev/gyro1", gyro1}, {NULL, NULL, NULL} }; @@ -137,7 +139,7 @@ accel(int argc, char *argv[]) } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("MPU6K acceleration values out of range!"); + warnx("ACCEL1 acceleration values out of range!"); return ERROR; } @@ -149,20 +151,19 @@ accel(int argc, char *argv[]) } static int -mpu6k(int argc, char *argv[]) +accel1(int argc, char *argv[]) { - printf("\tMPU6K: test start\n"); + printf("\tACCEL1: test start\n"); fflush(stdout); int fd; struct accel_report buf; - struct gyro_report gyro_buf; int ret; - fd = open("/dev/accel_mpu6k", O_RDONLY); + fd = open("/dev/accel1", O_RDONLY); if (fd < 0) { - printf("\tMPU6K: open fail, run first.\n"); + printf("\tACCEL1: open fail, run or first.\n"); return ERROR; } @@ -173,26 +174,40 @@ mpu6k(int argc, char *argv[]) ret = read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { - printf("\tMPU6K: read1 fail (%d)\n", ret); + printf("\tACCEL1: read1 fail (%d)\n", ret); return ERROR; } else { - printf("\tMPU6K accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tACCEL1 accel: x:%8.4f\ty:%8.4f\tz:%8.4f m/s^2\n", (double)buf.x, (double)buf.y, (double)buf.z); } if (fabsf(buf.x) > 30.0f || fabsf(buf.y) > 30.0f || fabsf(buf.z) > 30.0f) { - warnx("MPU6K acceleration values out of range!"); + warnx("ACCEL1 acceleration values out of range!"); return ERROR; } /* Let user know everything is ok */ - printf("\tOK: MPU6K ACCEL passed all tests successfully\n"); + printf("\tOK: ACCEL1 passed all tests successfully\n"); close(fd); - fd = open("/dev/gyro_mpu6k", O_RDONLY); + + return OK; +} + +static int +gyro(int argc, char *argv[]) +{ + printf("\tGYRO: test start\n"); + fflush(stdout); + + int fd; + struct gyro_report buf; + int ret; + + fd = open("/dev/gyro", O_RDONLY); if (fd < 0) { - printf("\tMPU6K GYRO: open fail, run or first.\n"); + printf("\tGYRO: open fail, run or first.\n"); return ERROR; } @@ -200,37 +215,37 @@ mpu6k(int argc, char *argv[]) usleep(5000); /* read data - expect samples */ - ret = read(fd, &gyro_buf, sizeof(gyro_buf)); + ret = read(fd, &buf, sizeof(buf)); - if (ret != sizeof(gyro_buf)) { - printf("\tMPU6K GYRO: read fail (%d)\n", ret); + if (ret != sizeof(buf)) { + printf("\tGYRO: read fail (%d)\n", ret); return ERROR; } else { - printf("\tMPU6K GYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)gyro_buf.x, (double)gyro_buf.y, (double)gyro_buf.z); + printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } /* Let user know everything is ok */ - printf("\tOK: MPU6K GYRO passed all tests successfully\n"); + printf("\tOK: GYRO passed all tests successfully\n"); close(fd); return OK; } static int -gyro(int argc, char *argv[]) +gyro1(int argc, char *argv[]) { - printf("\tGYRO: test start\n"); + printf("\tGYRO1: test start\n"); fflush(stdout); int fd; struct gyro_report buf; int ret; - fd = open("/dev/gyro", O_RDONLY); + fd = open("/dev/gyro1", O_RDONLY); if (fd < 0) { - printf("\tGYRO: open fail, run or first.\n"); + printf("\tGYRO1: open fail, run or first.\n"); return ERROR; } @@ -241,15 +256,15 @@ gyro(int argc, char *argv[]) ret = read(fd, &buf, sizeof(buf)); if (ret != sizeof(buf)) { - printf("\tGYRO: read fail (%d)\n", ret); + printf("\tGYRO1: read fail (%d)\n", ret); return ERROR; } else { - printf("\tGYRO rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); + printf("\tGYRO1 rates: x:%8.4f\ty:%8.4f\tz:%8.4f rad/s\n", (double)buf.x, (double)buf.y, (double)buf.z); } /* Let user know everything is ok */ - printf("\tOK: GYRO passed all tests successfully\n"); + printf("\tOK: GYRO1 passed all tests successfully\n"); close(fd); return OK; -- cgit v1.2.3 From 3ad9dd030c01e233a78aebfd2e20e67168962255 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 21:10:33 +0100 Subject: Added performance counter for write IOCTL --- src/drivers/px4io/px4io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e898b3e60..9e84bf929 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -244,7 +244,8 @@ private: int _mavlink_fd; /// 0) { + + perf_begin(_perf_write); int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + perf_end(_perf_write); if (ret != OK) return ret; -- cgit v1.2.3 From f174ca3ce5dfe338b79f52de46f127abf1c3aca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Dec 2013 21:52:10 +0100 Subject: Added average as direct output --- src/modules/systemlib/perf_counter.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index bf84b7945..b4ca0ed3e 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -295,10 +295,11 @@ perf_print_counter(perf_counter_t handle) case PC_ELAPSED: { struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; - printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n", + printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n", handle->name, pce->event_count, pce->time_total, + pce->time_total / pce->event_count, pce->time_least, pce->time_most); break; -- cgit v1.2.3 From 0f0dc5ba068d24fb8b339acc8ef850f5f6ea9e47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 12:45:04 +0100 Subject: Allowed custom battery scaling on IO --- src/drivers/px4io/px4io.cpp | 17 +++++++++++++++-- src/modules/sensors/sensor_params.c | 1 + 2 files changed, 16 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9e84bf929..df96fa0bb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -896,8 +896,21 @@ PX4IO::task_main() /* re-upload RC input config as it may have changed */ io_set_rc_config(); - } - } + + /* re-set the battery scaling */ + int32_t voltage_scaling_val; + param_t voltage_scaling_param; + + /* see if bind parameter has been set, and reset it to -1 */ + param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); + + /* send channel config to IO */ + uint16_t scaling = voltage_scaling_val; + int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); + + if (pret != OK) { + log("voltage scaling upload failed"); + } perf_end(_perf_update); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2aa15420a..78d4b410a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -195,6 +195,7 @@ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -- cgit v1.2.3 From 3e037d40de2a68b99aa4600f060eab3555f75832 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 12:46:06 +0100 Subject: Fixed bracketing error --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df96fa0bb..a7f7fce45 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -901,16 +901,19 @@ PX4IO::task_main() int32_t voltage_scaling_val; param_t voltage_scaling_param; - /* see if bind parameter has been set, and reset it to -1 */ + /* set battery voltage scaling */ param_get(voltage_scaling_param = param_find("BAT_V_SCALE_IO"), &voltage_scaling_val); - /* send channel config to IO */ + /* send scaling voltage to IO */ uint16_t scaling = voltage_scaling_val; int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { log("voltage scaling upload failed"); } + } + + } perf_end(_perf_update); } -- cgit v1.2.3 From b2e527ffa6f24a67903048ea157ee572f59f98a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 16:13:04 +0100 Subject: Counting channel count changes --- src/drivers/px4io/px4io.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a7f7fce45..b80844c5b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -237,6 +237,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values + unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels volatile int _task; /// 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); -- cgit v1.2.3 From 831f153b7385fecb180c977727eb6b2f8bef1317 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 21 Dec 2013 16:37:45 +0100 Subject: Add tight RC test --- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_rc.c | 146 ++++++++++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 150 insertions(+), 1 deletion(-) create mode 100644 src/systemcmds/tests/test_rc.c (limited to 'src') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 5d5fe50d3..68a080c77 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -27,4 +27,5 @@ SRCS = test_adc.c \ test_file.c \ tests_main.c \ test_param.c \ - test_ppm_loopback.c + test_ppm_loopback.c \ + test_rc.c diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c new file mode 100644 index 000000000..72619fc8b --- /dev/null +++ b/src/systemcmds/tests/test_rc.c @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 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 test_ppm_loopback.c + * Tests the PWM outputs and PPM input + * + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +int test_rc(int argc, char *argv[]) +{ + + int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + struct rc_input_values rc_input; + struct rc_input_values rc_last; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + usleep(100000); + + /* open PPM input and expect values close to the output values */ + + bool rc_updated; + orb_check(_rc_sub, &rc_updated); + + warnx("Reading PPM values - press any key to abort"); + warnx("This test guarantees: 10 Hz update rates, no glitches (channel values), no channel count changes."); + + if (rc_updated) { + + /* copy initial set */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + rc_last.values[i] = rc_input.values[i]; + } + + rc_last.channel_count = rc_input.channel_count; + + /* poll descriptor */ + struct pollfd fds[2]; + fds[0].fd = _rc_sub; + fds[0].events = POLLIN; + fds[1].fd = 0; + fds[1].events = POLLIN; + + while (true) { + + int ret = poll(fds, 2, 200); + + if (ret > 0) { + + if (fds[0].revents & POLLIN) { + + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* go and check values */ + for (unsigned i = 0; i < rc_input.channel_count; i++) { + if (fabsf(rc_input.values[i] - rc_last.values[i]) > 20) { + warnx("comparison fail: RC: %d, expected: %d", rc_input.values[i], rc_last.values[i]); + (void)close(_rc_sub); + return ERROR; + } + + rc_last.values[i] = rc_input.values[i]; + } + + if (rc_last.channel_count != rc_input.channel_count) { + warnx("channel count mismatch: last: %d, now: %d", rc_last.channel_count, rc_input.channel_count); + (void)close(_rc_sub); + return ERROR; + } + + if (hrt_absolute_time() - rc_input.timestamp > 100000) { + warnx("TIMEOUT, less than 10 Hz updates"); + (void)close(_rc_sub); + return ERROR; + } + + } else { + /* key pressed, bye bye */ + return 0; + } + + } + } + + } else { + warnx("failed reading RC input data"); + return ERROR; + } + + warnx("PPM CONTINUITY TEST PASSED SUCCESSFULLY!"); + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 5cbc5ad88..a57d04be3 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -108,6 +108,7 @@ extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); +extern int test_rc(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index cd998dd18..1088a4407 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -105,6 +105,7 @@ const struct { {"bson", test_bson, 0}, {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; -- cgit v1.2.3