From 496127ca459c603e5de3f8bc83c6113bcf9cbead Mon Sep 17 00:00:00 2001 From: sergeil Date: Fri, 31 May 2013 11:44:20 +0200 Subject: mpu6000 driver support for setting rate --- src/drivers/mpu6000/mpu6000.cpp | 30 ++++++++++++++++++++++++------ 1 file changed, 24 insertions(+), 6 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index df1958186..220842536 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 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 @@ -272,6 +272,11 @@ private: */ void _set_dlpf_filter(uint16_t frequency_hz); + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(uint16_t desired_sample_rate_hz); + }; /** @@ -378,7 +383,8 @@ MPU6000::init() up_udelay(1000); // SAMPLE RATE - write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz + _set_sample_rate(200); // default sample rate = 200Hz usleep(1000); // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) @@ -493,6 +499,18 @@ MPU6000::probe() return -EIO; } +/* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro +*/ +void +MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) +{ + uint8_t div = 1000 / desired_sample_rate_hz; + if(div>200) div=200; + if(div<1) div=1; + write_reg(MPUREG_SMPLRT_DIV, div-1); +} + /* set the DLPF filter frequency. This affects both accel and gyro. */ @@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSAMPLERATE: case ACCELIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: @@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSAMPLERATE: case GYROIOCGSAMPLERATE: - /* XXX not implemented */ - return -EINVAL; + _set_sample_rate(arg); + return OK; case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: -- cgit v1.2.3 From e3bb9e87e2af2f0f70e486405e2c6df5d3e23667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 17:36:24 +0200 Subject: Hotfix for GPS: Disable unknown message classes --- src/drivers/gps/ubx.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b46089073 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,7 +176,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 1); + 0); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -327,6 +327,7 @@ UBX::parse_char(uint8_t b) } break; case UBX_DECODE_GOT_CLASS: + { add_byte_to_checksum(b); switch (_message_class) { case NAV: @@ -413,6 +414,14 @@ UBX::parse_char(uint8_t b) // config_needed = true; break; } + // Evaluate state machine - if the state changed, + // the state machine was reset via decode_init() + // and we want to tell the module to stop sending this message + + // disable unknown message + warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + configure_message_rate(_message_class, b, 0); + } break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); -- cgit v1.2.3 From 6cbbb9b99fbeddc2da00f67bb209d33971a30041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:46:24 +0200 Subject: Hotfix for GPS driver --- src/drivers/gps/ubx.cpp | 51 +++++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 25 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b46089073..3e790499b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -224,35 +224,18 @@ UBX::receive(unsigned timeout) fds[0].fd = _fd; fds[0].events = POLLIN; - uint8_t buf[32]; + uint8_t buf[128]; /* timeout additional to poll */ uint64_t time_started = hrt_absolute_time(); - int j = 0; ssize_t count = 0; - while (true) { - - /* pass received bytes to the packet decoder */ - while (j < count) { - if (parse_char(buf[j]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ - if (handle_message() > 0) - return 1; - } - /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { - return -1; - } - j++; - } + bool message_found = false; - /* everything is read */ - j = count = 0; + while (true) { - /* then poll for new data */ + /* poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { @@ -272,8 +255,26 @@ UBX::receive(unsigned timeout) * available, we'll go back to poll() again... */ count = ::read(_fd, buf, sizeof(buf)); + /* pass received bytes to the packet decoder */ + for (int i = 0; i < count; i++) { + if (parse_char(buf[i])) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + handle_message(); + message_found = true; + } + } } } + + /* return success after receiving a packet */ + if (message_found) + return 1; + + /* abort after timeout if no packet parsed successfully */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } } } -- cgit v1.2.3 From c3d07030dd1882c626ed027cfc5870f42b1cd33e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:56:02 +0200 Subject: Minor additions to fix, pushing --- src/drivers/gps/ubx.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 3e790499b..ad219cd25 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -231,7 +231,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool message_found = false; + bool position_updated = false; while (true) { @@ -260,15 +260,15 @@ UBX::receive(unsigned timeout) if (parse_char(buf[i])) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - handle_message(); - message_found = true; + if (handle_message()) + position_updated = true; } } } } /* return success after receiving a packet */ - if (message_found) + if (position_updated) return 1; /* abort after timeout if no packet parsed successfully */ @@ -420,8 +420,8 @@ UBX::parse_char(uint8_t b) // and we want to tell the module to stop sending this message // disable unknown message - warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - configure_message_rate(_message_class, b, 0); + //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + //configure_message_rate(_message_class, b, 0); } break; case UBX_DECODE_GOT_MESSAGEID: -- cgit v1.2.3