diff --git a/CMakeLists.txt b/CMakeLists.txt index e8427d8..8a52ae2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(um7) -find_package(catkin REQUIRED COMPONENTS roscpp serial roslint sensor_msgs - message_generation) +find_package(catkin REQUIRED COMPONENTS roscpp serial sensor_msgs message_generation) add_service_files( FILES @@ -31,7 +30,6 @@ add_executable(um7_driver src/main.cpp src/registers.cpp src/comms.cpp) target_link_libraries(um7_driver ${catkin_LIBRARIES} ) add_dependencies(um7_driver um7_generate_messages_cpp) - ############# ## Install ## ############# @@ -40,28 +38,20 @@ install(TARGETS um7_driver RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -########## -## Docs ## -########## - -file(GLOB LINT_SRCS - src/*.cpp - include/um6/registers.h - include/um6/comms.h) -roslint_cpp(${LINT_SRCS}) -roslint_add_test() - ############# ## Testing ## ############# +if(CATKIN_ENABLE_TESTING) + find_package(roslint REQUIRED) -catkin_add_gtest(${PROJECT_NAME}_test_registers test/test_registers.cpp src/registers.cpp) -catkin_add_gtest(${PROJECT_NAME}_test_comms test/test_comms.cpp src/comms.cpp src/registers.cpp) -if(TARGET ${PROJECT_NAME}_test_comms) + catkin_add_gtest(${PROJECT_NAME}_test_registers test/test_registers.cpp src/registers.cpp) + catkin_add_gtest(${PROJECT_NAME}_test_comms test/test_comms.cpp src/comms.cpp src/registers.cpp) target_link_libraries(${PROJECT_NAME}_test_comms util ${catkin_LIBRARIES}) -endif() -if(TARGET ${PROJECT_NAME}_test_registers) - target_link_libraries(${PROJECT_NAME}_test_registers util ${catkin_LIBRARIES}) -endif() - + file(GLOB LINT_SRCS + src/*.cpp + include/um7/registers.h + include/um7/comms.h) + roslint_cpp(${LINT_SRCS}) + roslint_add_test() +endif() diff --git a/include/um7/comms.h b/include/um7/comms.h index 94349f9..1102045 100644 --- a/include/um7/comms.h +++ b/include/um7/comms.h @@ -16,7 +16,7 @@ * * Neither the name of Clearpath Robotics, Inc. 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 @@ -27,22 +27,24 @@ * 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. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Please send comments, questions, or patches to code@clearpathrobotics.com * */ -#ifndef INCLUDE_UM7_COMMS_H_ -#define INCLUDE_UM7_COMMS_H_ +#ifndef UM7_COMMS_H_ +#define UM7_COMMS_H_ #include #include -namespace serial { - class Serial; +namespace serial +{ +class Serial; } -namespace um7 { +namespace um7 +{ class SerialTimeout : public std::exception {}; @@ -51,37 +53,39 @@ class BadChecksum : public std::exception {}; class Registers; class Accessor_; -class Comms { - public: - explicit Comms(serial::Serial* s) : serial_(s), first_spin_(true) { - } +class Comms +{ +public: + explicit Comms(serial::Serial* s) : serial_(s), first_spin_(true) + { + } - /** - * Returns -1 if the serial port timed out before receiving a packet - * successfully, or if there was a bad checksum or any other error. - * Otherwise, returns the 8-bit register number of the successfully - * returned packet. - */ - int16_t receive(Registers* r); + /** + * Returns -1 if the serial port timed out before receiving a packet + * successfully, or if there was a bad checksum or any other error. + * Otherwise, returns the 8-bit register number of the successfully + * returned packet. + */ + int16_t receive(Registers* r); - void send(const Accessor_& a) const; + void send(const Accessor_& a) const; - bool sendWaitAck(const Accessor_& a); + bool sendWaitAck(const Accessor_& a); - static const uint8_t PACKET_HAS_DATA; - static const uint8_t PACKET_IS_BATCH; - static const uint8_t PACKET_BATCH_LENGTH_MASK; - static const uint8_t PACKET_BATCH_LENGTH_OFFSET; + static const uint8_t PACKET_HAS_DATA; + static const uint8_t PACKET_IS_BATCH; + static const uint8_t PACKET_BATCH_LENGTH_MASK; + static const uint8_t PACKET_BATCH_LENGTH_OFFSET; - static std::string checksum(const std::string& s); + static std::string checksum(const std::string& s); - static std::string message(uint8_t address, std::string data); + static std::string message(uint8_t address, std::string data); - private: - bool first_spin_; - serial::Serial* serial_; +private: + bool first_spin_; + serial::Serial* serial_; }; -} +} // namespace um7 -#endif // INCLUDE_UM7_COMMS_H_ +#endif // UM7_COMMS_H diff --git a/include/um7/firmware_registers.h b/include/um7/firmware_registers.h index 09cf3c6..899625a 100644 --- a/include/um7/firmware_registers.h +++ b/include/um7/firmware_registers.h @@ -1,19 +1,19 @@ - /** + /** * \file * \brief Copied directly from the UM7 version of the UM6_config.h file, available online here: * http://sourceforge.net/p/um6firmware/code/34/tree/trunk/UM6%20Firmware/UM6_config.h#l14 * Note: while while the source of this code is named "UM6_config.h", it is not the same * as the file used for the UM6 and is part of the UM7 source code. * \author Alex Brown rbirac@cox.net - * \maintainer Alex Brown rbirac@cox.net + * \maintainer Alex Brown rbirac@cox.net */ // Define the firmware revision -#define UM6_FIRMWARE_REVISION (('U' << 24) | ('7' << 16) | ('1' << 8) | 'C') +#define UM7_FIRMWARE_REVISION (('U' << 24) | ('7' << 16) | ('1' << 8) | 'C') // CONFIG_ARRAY_SIZE and DATA_ARRAY_SIZE specify the number of 32 bit configuration and data registers used by the firmware -// (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a +// (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a // microcontroller register. They are simply index locations into arrays stored in global memory. Data and configuration // parameters are stored in arrays because it allows a common communication protocol to be used to access all data and // configuration. The software communicating with the sensor needs only specify the register address, and the communication @@ -21,273 +21,270 @@ // with the sensor, on the other hand, needs to know what it is asking for (naturally...) // This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in // the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side. -#define CONFIG_ARRAY_SIZE 27 -#define DATA_ARRAY_SIZE 52 -#define COMMAND_COUNT 10 +#define CONFIG_ARRAY_SIZE 27 +#define DATA_ARRAY_SIZE 52 +#define COMMAND_COUNT 10 -#define HIDDEN_ARRAY_SIZE 127 +#define HIDDEN_ARRAY_SIZE 127 // Define address ranges for interpreting incoming packet data -#define CONFIG_REG_START_ADDRESS 0 -#define DATA_REG_START_ADDRESS 85 -#define COMMAND_START_ADDRESS 170 +#define CONFIG_REG_START_ADDRESS 0 +#define DATA_REG_START_ADDRESS 85 +#define COMMAND_START_ADDRESS 170 // Definitions of configuration registers/address -#define CREG_COM_SETTINGS 0 -#define CREG_COM_RATES1 1 -#define CREG_COM_RATES2 2 -#define CREG_COM_RATES3 3 -#define CREG_COM_RATES4 4 -#define CREG_COM_RATES5 5 -#define CREG_COM_RATES6 6 -#define CREG_COM_RATES7 7 -#define CREG_MISC_SETTINGS 8 -#define CREG_HOME_NORTH 9 -#define CREG_HOME_EAST 10 -#define CREG_HOME_UP 11 -#define CREG_GYRO_TRIM_X 12 // Floating point trim (actual angular rate) -#define CREG_GYRO_TRIM_Y 13 -#define CREG_GYRO_TRIM_Z 14 -#define CREG_MAG_CAL1_1 15 -#define CREG_MAG_CAL1_2 16 -#define CREG_MAG_CAL1_3 17 -#define CREG_MAG_CAL2_1 18 -#define CREG_MAG_CAL2_2 19 -#define CREG_MAG_CAL2_3 20 -#define CREG_MAG_CAL3_1 21 -#define CREG_MAG_CAL3_2 22 -#define CREG_MAG_CAL3_3 23 -#define CREG_MAG_BIAS_X 24 -#define CREG_MAG_BIAS_Y 25 -#define CREG_MAG_BIAS_Z 26 +#define CREG_COM_SETTINGS 0 +#define CREG_COM_RATES1 1 +#define CREG_COM_RATES2 2 +#define CREG_COM_RATES3 3 +#define CREG_COM_RATES4 4 +#define CREG_COM_RATES5 5 +#define CREG_COM_RATES6 6 +#define CREG_COM_RATES7 7 +#define CREG_MISC_SETTINGS 8 +#define CREG_HOME_NORTH 9 +#define CREG_HOME_EAST 10 +#define CREG_HOME_UP 11 +#define CREG_GYRO_TRIM_X 12 // Floating point trim (actual angular rate) +#define CREG_GYRO_TRIM_Y 13 +#define CREG_GYRO_TRIM_Z 14 +#define CREG_MAG_CAL1_1 15 +#define CREG_MAG_CAL1_2 16 +#define CREG_MAG_CAL1_3 17 +#define CREG_MAG_CAL2_1 18 +#define CREG_MAG_CAL2_2 19 +#define CREG_MAG_CAL2_3 20 +#define CREG_MAG_CAL3_1 21 +#define CREG_MAG_CAL3_2 22 +#define CREG_MAG_CAL3_3 23 +#define CREG_MAG_BIAS_X 24 +#define CREG_MAG_BIAS_Y 25 +#define CREG_MAG_BIAS_Z 26 // Bit definitions for COM_SETTINGS register -#define COM_BAUD_MASK 0x0F // Uses 4 bits -#define COM_BAUD_START 28 // Lowest-order bit on bit 28 +#define COM_BAUD_MASK 0x0F // Uses 4 bits +#define COM_BAUD_START 28 // Lowest-order bit on bit 28 -#define COM_GPS_BAUD_MASK 0x0F -#define COM_GPS_BAUD_START 24 +#define COM_GPS_BAUD_MASK 0x0F +#define COM_GPS_BAUD_START 24 -#define COM_GPS_DATA_ENABLED (1 << 8) -#define COM_GPS_SAT_DATA_ENABLED (1 << 4) +#define COM_GPS_DATA_ENABLED (1 << 8) +#define COM_GPS_SAT_DATA_ENABLED (1 << 4) // Definitions for controlling the baud rate of the USART -#define BAUD_9600 0 -#define BAUD_14400 1 -#define BAUD_19200 2 -#define BAUD_38400 3 -#define BAUD_57600 4 -#define BAUD_115200 5 -#define BAUD_128000 6 -#define BAUD_153600 7 -#define BAUD_230400 8 -#define BAUD_256000 9 -#define BAUD_460800 10 -#define BAUD_921600 11 +#define BAUD_9600 0 +#define BAUD_14400 1 +#define BAUD_19200 2 +#define BAUD_38400 3 +#define BAUD_57600 4 +#define BAUD_115200 5 +#define BAUD_128000 6 +#define BAUD_153600 7 +#define BAUD_230400 8 +#define BAUD_256000 9 +#define BAUD_460800 10 +#define BAUD_921600 11 // Bit definitions for COM_RATE registers -#define RATE1_RAW_ACCEL_MASK 0x0FF -#define RATE1_RAW_ACCEL_START 24 -#define RATE1_RAW_GYRO_MASK 0x0FF -#define RATE1_RAW_GYRO_START 16 -#define RATE1_RAW_MAG_MASK 0x0FF -#define RATE1_RAW_MAG_START 8 - -#define RATE2_TEMPERATURE_MASK 0x0FF -#define RATE2_TEMPERATURE_START 24 -#define RATE2_ALL_RAW_MASK 0x0FF -#define RATE2_ALL_RAW_START 0 - -#define RATE3_PROC_ACCEL_MASK 0x0FF -#define RATE3_PROC_ACCEL_START 24 -#define RATE3_PROC_GYRO_MASK 0x0FF -#define RATE3_PROC_GYRO_START 16 -#define RATE3_PROC_MAG_MASK 0x0FF -#define RATE3_PROC_MAG_START 8 - -#define RATE4_ALL_PROC_MASK 0x0FF -#define RATE4_ALL_PROC_START 0 - -#define RATE5_QUAT_MASK 0x0FF -#define RATE5_QUAT_START 24 -#define RATE5_EULER_MASK 0x0FF -#define RATE5_EULER_START 16 -#define RATE5_POSITION_MASK 0x0FF -#define RATE5_POSITION_START 8 -#define RATE5_VELOCITY_MASK 0x0FF -#define RATE5_VELOCITY_START 0 - -#define RATE6_POSE_MASK 0x0FF -#define RATE6_POSE_START 24 -#define RATE6_HEALTH_MASK 0x0F -#define RATE6_HEALTH_START 16 - -#define RATE7_NMEA_HEALTH_MASK 0x0F -#define RATE7_NMEA_HEALTH_START 28 -#define RATE7_NMEA_POSE_MASK 0x0F -#define RATE7_NMEA_POSE_START 24 -#define RATE7_NMEA_ATTITUDE_MASK 0x0F -#define RATE7_NMEA_ATTITUDE_START 20 -#define RATE7_NMEA_SENSOR_MASK 0x0F -#define RATE7_NMEA_SENSOR_START 16 -#define RATE7_NMEA_RATE_MASK 0x0F -#define RATE7_NMEA_RATE_START 12 -#define RATE7_NMEA_GPS_POSE_MASK 0x0F -#define RATE7_NMEA_GPS_POSE_START 8 -#define RATE7_NMEA_QUATERNION_MASK 0x0F -#define RATE7_NMEA_QUATERNION_START 4 +#define RATE1_RAW_ACCEL_MASK 0x0FF +#define RATE1_RAW_ACCEL_START 24 +#define RATE1_RAW_GYRO_MASK 0x0FF +#define RATE1_RAW_GYRO_START 16 +#define RATE1_RAW_MAG_MASK 0x0FF +#define RATE1_RAW_MAG_START 8 + +#define RATE2_TEMPERATURE_MASK 0x0FF +#define RATE2_TEMPERATURE_START 24 +#define RATE2_ALL_RAW_MASK 0x0FF +#define RATE2_ALL_RAW_START 0 + +#define RATE3_PROC_ACCEL_MASK 0x0FF +#define RATE3_PROC_ACCEL_START 24 +#define RATE3_PROC_GYRO_MASK 0x0FF +#define RATE3_PROC_GYRO_START 16 +#define RATE3_PROC_MAG_MASK 0x0FF +#define RATE3_PROC_MAG_START 8 + +#define RATE4_ALL_PROC_MASK 0x0FF +#define RATE4_ALL_PROC_START 0 + +#define RATE5_QUAT_MASK 0x0FF +#define RATE5_QUAT_START 24 +#define RATE5_EULER_MASK 0x0FF +#define RATE5_EULER_START 16 +#define RATE5_POSITION_MASK 0x0FF +#define RATE5_POSITION_START 8 +#define RATE5_VELOCITY_MASK 0x0FF +#define RATE5_VELOCITY_START 0 + +#define RATE6_POSE_MASK 0x0FF +#define RATE6_POSE_START 24 +#define RATE6_HEALTH_MASK 0x0F +#define RATE6_HEALTH_START 16 + +#define RATE7_NMEA_HEALTH_MASK 0x0F +#define RATE7_NMEA_HEALTH_START 28 +#define RATE7_NMEA_POSE_MASK 0x0F +#define RATE7_NMEA_POSE_START 24 +#define RATE7_NMEA_ATTITUDE_MASK 0x0F +#define RATE7_NMEA_ATTITUDE_START 20 +#define RATE7_NMEA_SENSOR_MASK 0x0F +#define RATE7_NMEA_SENSOR_START 16 +#define RATE7_NMEA_RATE_MASK 0x0F +#define RATE7_NMEA_RATE_START 12 +#define RATE7_NMEA_GPS_POSE_MASK 0x0F +#define RATE7_NMEA_GPS_POSE_START 8 +#define RATE7_NMEA_QUATERNION_MASK 0x0F +#define RATE7_NMEA_QUATERNION_START 4 // Bit definitions for filter settings register. -#define MAG_UPDATES_ENABLED (1 << 0) +#define MAG_UPDATES_ENABLED (1 << 0) #define QUATERNION_MODE_ENABLED (1 << 1) #define ZERO_GYROS_ON_STARTUP (1 << 2) -#define USE_TX2_AS_PPS_INPUT (1 << 8) +#define USE_TX2_AS_PPS_INPUT (1 << 8) // Definitions of data registers/addresses -#define DREG_HEALTH 85 -#define DREG_GYRO_RAW_XY 86 -#define DREG_GYRO_RAW_Z 87 -#define DREG_GYRO_TIME 88 -#define DREG_ACCEL_RAW_XY 89 -#define DREG_ACCEL_RAW_Z 90 -#define DREG_ACCEL_TIME 91 -#define DREG_MAG_RAW_XY 92 -#define DREG_MAG_RAW_Z 93 -#define DREG_MAG_RAW_TIME 94 -#define DREG_TEMPERATURE 95 -#define DREG_TEMPERATURE_TIME 96 -#define DREG_GYRO_PROC_X 97 -#define DREG_GYRO_PROC_Y 98 -#define DREG_GYRO_PROC_Z 99 -#define DREG_GYRO_PROC_TIME 100 -#define DREG_ACCEL_PROC_X 101 -#define DREG_ACCEL_PROC_Y 102 -#define DREG_ACCEL_PROC_Z 103 -#define DREG_ACCEL_PROC_TIME 104 -#define DREG_MAG_PROC_X 105 -#define DREG_MAG_PROC_Y 106 -#define DREG_MAG_PROC_Z 107 -#define DREG_MAG_PROC_TIME 108 -#define DREG_QUAT_AB 109 -#define DREG_QUAT_CD 110 -#define DREG_QUAT_TIME 111 -#define DREG_EULER_PHI_THETA 112 -#define DREG_EULER_PSI 113 -#define DREG_EULER_PHI_THETA_DOT 114 -#define DREG_EULER_PSI_DOT 115 -#define DREG_EULER_TIME 116 -#define DREG_POSITION_NORTH 117 -#define DREG_POSITION_EAST 118 -#define DREG_POSITION_UP 119 -#define DREG_POSITION_TIME 120 -#define DREG_VELOCITY_NORTH 121 -#define DREG_VELOCITY_EAST 122 -#define DREG_VELOCITY_UP 123 -#define DREG_VELOCITY_TIME 124 -#define DREG_GPS_LATITUDE 125 -#define DREG_GPS_LONGITUDE 126 -#define DREG_GPS_ALTITUDE 127 -#define DREG_GPS_COURSE 128 -#define DREG_GPS_SPEED 129 -#define DREG_GPS_TIME 130 -#define DREG_GPS_SAT_1_2 131 -#define DREG_GPS_SAT_3_4 132 -#define DREG_GPS_SAT_5_6 133 -#define DREG_GPS_SAT_7_8 134 -#define DREG_GPS_SAT_9_10 135 -#define DREG_GPS_SAT_11_12 136 +#define DREG_HEALTH 85 +#define DREG_GYRO_RAW_XY 86 +#define DREG_GYRO_RAW_Z 87 +#define DREG_GYRO_TIME 88 +#define DREG_ACCEL_RAW_XY 89 +#define DREG_ACCEL_RAW_Z 90 +#define DREG_ACCEL_TIME 91 +#define DREG_MAG_RAW_XY 92 +#define DREG_MAG_RAW_Z 93 +#define DREG_MAG_RAW_TIME 94 +#define DREG_TEMPERATURE 95 +#define DREG_TEMPERATURE_TIME 96 +#define DREG_GYRO_PROC_X 97 +#define DREG_GYRO_PROC_Y 98 +#define DREG_GYRO_PROC_Z 99 +#define DREG_GYRO_PROC_TIME 100 +#define DREG_ACCEL_PROC_X 101 +#define DREG_ACCEL_PROC_Y 102 +#define DREG_ACCEL_PROC_Z 103 +#define DREG_ACCEL_PROC_TIME 104 +#define DREG_MAG_PROC_X 105 +#define DREG_MAG_PROC_Y 106 +#define DREG_MAG_PROC_Z 107 +#define DREG_MAG_PROC_TIME 108 +#define DREG_QUAT_AB 109 +#define DREG_QUAT_CD 110 +#define DREG_QUAT_TIME 111 +#define DREG_EULER_PHI_THETA 112 +#define DREG_EULER_PSI 113 +#define DREG_EULER_PHI_THETA_DOT 114 +#define DREG_EULER_PSI_DOT 115 +#define DREG_EULER_TIME 116 +#define DREG_POSITION_NORTH 117 +#define DREG_POSITION_EAST 118 +#define DREG_POSITION_UP 119 +#define DREG_POSITION_TIME 120 +#define DREG_VELOCITY_NORTH 121 +#define DREG_VELOCITY_EAST 122 +#define DREG_VELOCITY_UP 123 +#define DREG_VELOCITY_TIME 124 +#define DREG_GPS_LATITUDE 125 +#define DREG_GPS_LONGITUDE 126 +#define DREG_GPS_ALTITUDE 127 +#define DREG_GPS_COURSE 128 +#define DREG_GPS_SPEED 129 +#define DREG_GPS_TIME 130 +#define DREG_GPS_SAT_1_2 131 +#define DREG_GPS_SAT_3_4 132 +#define DREG_GPS_SAT_5_6 133 +#define DREG_GPS_SAT_7_8 134 +#define DREG_GPS_SAT_9_10 135 +#define DREG_GPS_SAT_11_12 136 // Bit definitions for sensor health register -#define HEALTH_SATS_USED_MASK 0x3F // Uses 6 bits -#define HEALTH_SATS_USED_START 26 // Lowest-order bit starts at 26 -#define HEALTH_HDOP_MASK 0x3FF // Uses 10 bits -#define HEALTH_HDOP_START 16 // Lowest-order bit starts at 16 -#define HEALTH_SATS_IN_VIEW_MASK 0x3F -#define HEALTH_SATS_IN_VIEW_START 10 -#define HEALTH_COM_OVERFLOW (1 << 8) // Set when the sensor was unable to transmit all the requested data -#define HEALTH_MAG_NORM (1 << 5) -#define HEALTH_ACCEL_NORM (1 << 4) -#define HEALTH_ACCEL (1 << 3) -#define HEALTH_GYRO (1 << 2) -#define HEALTH_MAG (1 << 1) -#define HEALTH_GPS (1 << 0) +#define HEALTH_SATS_USED_MASK 0x3F // Uses 6 bits +#define HEALTH_SATS_USED_START 26 // Lowest-order bit starts at 26 +#define HEALTH_HDOP_MASK 0x3FF // Uses 10 bits +#define HEALTH_HDOP_START 16 // Lowest-order bit starts at 16 +#define HEALTH_SATS_IN_VIEW_MASK 0x3F +#define HEALTH_SATS_IN_VIEW_START 10 +#define HEALTH_COM_OVERFLOW (1 << 8) // Set when the sensor was unable to transmit all the requested data +#define HEALTH_MAG_NORM (1 << 5) +#define HEALTH_ACCEL_NORM (1 << 4) +#define HEALTH_ACCEL (1 << 3) +#define HEALTH_GYRO (1 << 2) +#define HEALTH_MAG (1 << 1) +#define HEALTH_GPS (1 << 0) // Definition of packet address for COM error codes -#define CHR_BAD_CHECKSUM 253 // Sent if the module receives a packet with a bad checksum -#define CHR_UNKNOWN_ADDRESS 254 // Sent if the module receives a packet with an unknown address -#define CHR_INVALID_BATCH_SIZE 255 // Sent if a requested batch read or write operation would go beyond the bounds of the config or data array +#define CHR_BAD_CHECKSUM 253 // Sent if the module receives a packet with a bad checksum +#define CHR_UNKNOWN_ADDRESS 254 // Sent if the module receives a packet with an unknown address +#define CHR_INVALID_BATCH_SIZE 255 // Sent if a requested batch read or write operation would go beyond the bounds of the config or data array // Command addresses -#define CHR_GET_FW_VERSION COMMAND_START_ADDRESS // Causes the device to report the firmware revision -#define CHR_FLASH_COMMIT (COMMAND_START_ADDRESS + 1) // Causes the device to write all configuration values to FLASH -#define CHR_RESET_TO_FACTORY (COMMAND_START_ADDRESS + 2) // Causes the UM6 to load default factory settings -#define CHR_ZERO_GYROS (COMMAND_START_ADDRESS + 3) -#define CHR_SET_HOME_POSITION (COMMAND_START_ADDRESS + 4) -#define CHR_FACTORY_COMMIT (COMMAND_START_ADDRESS + 5) -#define CHR_SET_MAG_REFERENCE (COMMAND_START_ADDRESS + 6) - -#define CHR_RESET_EKF (COMMAND_START_ADDRESS + 9) +#define CHR_GET_FW_VERSION (COMMAND_START_ADDRESS + 0) // Causes the device to report the firmware revision +#define CHR_FLASH_COMMIT (COMMAND_START_ADDRESS + 1) // Causes the device to write all configuration values to FLASH +#define CHR_RESET_TO_FACTORY (COMMAND_START_ADDRESS + 2) // Causes the UM6 to load default factory settings +#define CHR_ZERO_GYROS (COMMAND_START_ADDRESS + 3) +#define CHR_SET_HOME_POSITION (COMMAND_START_ADDRESS + 4) +#define CHR_FACTORY_COMMIT (COMMAND_START_ADDRESS + 5) +#define CHR_SET_MAG_REFERENCE (COMMAND_START_ADDRESS + 6) +#define CHR_RESET_EKF (COMMAND_START_ADDRESS + 9) // Some definitions for working with COM events -#define NMEA_HEALTH_PACKET 0 -#define NMEA_POSE_PACKET 1 -#define NMEA_ATTITUDE_PACKET 2 -#define NMEA_SENSOR_PACKET 3 -#define NMEA_RATE_PACKET 4 -#define NMEA_GPS_POSE_PACKET 5 -#define NMEA_QUATERNION_PACKET 6 +#define NMEA_HEALTH_PACKET 0 +#define NMEA_POSE_PACKET 1 +#define NMEA_ATTITUDE_PACKET 2 +#define NMEA_SENSOR_PACKET 3 +#define NMEA_RATE_PACKET 4 +#define NMEA_GPS_POSE_PACKET 5 +#define NMEA_QUATERNION_PACKET 6 // Definition for zeroing rate gyros and pressure sensor -#define GYRO_ZERO_SAMPLES 500 -#define GYRO_X_INDEX 0 -#define GYRO_Y_INDEX 1 -#define GYRO_Z_INDEX 2 -#define ANGLE_ZERO_SAMPLES 1000 -#define ACCEL_X_INDEX 0 -#define ACCEL_Y_INDEX 1 -#define ACCEL_Z_INDEX 2 -#define PRESSURE_ZERO_SAMPLES 250 -#define AIRSPEED_ZERO_SAMPLES 500 - -#define PI 3.14159265f -#define PIx2 6.28318530f -#define PId2 1.57079633f -#define GRAVITY 9.80665f -#define GRAVITYx2 19.6133f -#define GRAVITYd2 4.903325f -#define RAW_TO_MBAR (1.0f/4096.0f) -#define KPA_PER_VOLT 1.0f - -#define CHR_USE_CONFIG_ADDRESS 0 -#define CHR_USE_FACTORY_ADDRESS 1 +#define GYRO_ZERO_SAMPLES 500 +#define GYRO_X_INDEX 0 +#define GYRO_Y_INDEX 1 +#define GYRO_Z_INDEX 2 +#define ANGLE_ZERO_SAMPLES 1000 +#define ACCEL_X_INDEX 0 +#define ACCEL_Y_INDEX 1 +#define ACCEL_Z_INDEX 2 +#define PRESSURE_ZERO_SAMPLES 250 +#define AIRSPEED_ZERO_SAMPLES 500 + +#define PI 3.14159265f +#define PIx2 6.28318530f +#define PId2 1.57079633f +#define GRAVITY 9.80665f +#define GRAVITYx2 19.6133f +#define GRAVITYd2 4.903325f +#define RAW_TO_MBAR (1.0f/4096.0f) +#define KPA_PER_VOLT 1.0f + +#define CHR_USE_CONFIG_ADDRESS 0 +#define CHR_USE_FACTORY_ADDRESS 1 // Start address for writing configuration to FLASH -#define FLASH_START_ADDRESS (uint32_t)0x0800F000 +#define FLASH_START_ADDRESS (uint32_t)0x0800F000 // Start address for factory FLASH configuration -#define FACTORY_FLASH_ADDRESS (uint32_t)0x0800E000 +#define FACTORY_FLASH_ADDRESS (uint32_t)0x0800E000 // Macro for determining whether FLASH has been initialized -#define FGET_FLASH_UNINITIALIZED() ((uint32_t)( *(__IO uint32_t*)(FLASH_START_ADDRESS) ) == 0xFFFFFFFF) -#define FGET_FACTORY_UNINITIALIZED() ((uint32_t)( *(__IO uint32_t*)(FACTORY_FLASH_ADDRESS) ) == 0xFFFFFFFF) +#define FGET_FLASH_UNINITIALIZED() ((uint32_t)( *(__IO uint32_t*)(FLASH_START_ADDRESS) ) == 0xFFFFFFFF) +#define FGET_FACTORY_UNINITIALIZED() ((uint32_t)( *(__IO uint32_t*)(FACTORY_FLASH_ADDRESS) ) == 0xFFFFFFFF) -#define GYRO_ZERO_SAMPLE_SIZE 500 +#define GYRO_ZERO_SAMPLE_SIZE 500 -typedef struct __CHR_config +typedef struct __CHR_config { - union - { - uint32_t r[CONFIG_ARRAY_SIZE]; - float f[CONFIG_ARRAY_SIZE]; - }; + union + { + uint32_t r[CONFIG_ARRAY_SIZE]; + float f[CONFIG_ARRAY_SIZE]; + }; } CHR_config; typedef struct __CHR_data { - union - { - uint32_t r[DATA_ARRAY_SIZE]; - float f[DATA_ARRAY_SIZE]; - }; + union + { + uint32_t r[DATA_ARRAY_SIZE]; + float f[DATA_ARRAY_SIZE]; + }; } CHR_data; - - diff --git a/include/um7/registers.h b/include/um7/registers.h index 29da179..a77bd2b 100644 --- a/include/um7/registers.h +++ b/include/um7/registers.h @@ -8,7 +8,7 @@ * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. * \author Alex Brown adapted code for UM7 * \copyright Copyright (c) 2015, Alex Brown - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * Redistributions of source code must retain the above copyright @@ -19,7 +19,7 @@ * * Neither the name of Clearpath Robotics, Inc. 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 @@ -30,13 +30,11 @@ * 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. - * - * Please send comments, questions, or patches to Alex Brown rbirac@cox.net * */ -#ifndef INCLUDE_UM7_REGISTERS_H_ -#define INCLUDE_UM7_REGISTERS_H_ +#ifndef UM7_REGISTERS_H_ +#define UM7_REGISTERS_H_ #if __APPLE__ #include @@ -44,13 +42,12 @@ #include #endif -#include #include #include #include -#include #include +#include #include "um7/firmware_registers.h" @@ -62,9 +59,11 @@ #define NUM_REGISTERS (DATA_REG_START_ADDRESS + DATA_ARRAY_SIZE) -namespace um7 { +namespace um7 +{ -inline void memcpy_network(void* dest, void* src, size_t count) { +inline void memcpy_network(void* dest, void* src, size_t count) +{ #if __BYTE_ORDER == __LITTLE_ENDIAN uint8_t* d = reinterpret_cast(dest); uint8_t* s = reinterpret_cast(src); @@ -82,66 +81,72 @@ class Registers; /** * This class provides an accessor of fields contained in one or more - * consecutive UM7 registers. Each register is nominally a uint32_t, - * but XYZ vectors are sometimes stored as a pair of int16_t values in - * one register and one in the following register. Other values are + * consecutive UM7 registers. Each register is nominally a uint32_t, + * but XYZ vectors are sometimes stored as a pair of int16_t values in + * one register and one in the following register. Other values are * stored as int32_t representation or float32s. * * This class takes care of the necessary transformations to simplify * the actual "business logic" of the driver. */ -class Accessor_ { - public: - Accessor_(Registers* registers, uint8_t register_index, - uint8_t register_width, uint8_t array_length) - : index(register_index), width(register_width), - length(array_length), registers_(registers) - {} - - void* raw() const; - - /** - * Number/address of the register in the array of uint32s which is - * shared with the UM7 firmware. */ - const uint8_t index; - - /** - * Width of the sub-register field, in bytes, either 2 or 4. */ - const uint8_t width; - - /** - * Length of how many sub-register fields comprise this accessor. Not - * required to stay within the bounds of a single register. */ - const uint16_t length; - - private: - Registers* registers_; +class Accessor_ +{ +public: + Accessor_(Registers* registers, uint8_t register_index, + uint8_t register_width, uint8_t array_length) + : index(register_index), width(register_width), + length(array_length), registers_(registers) + {} + + void* raw() const; + + /** + * Number/address of the register in the array of uint32s which is + * shared with the UM7 firmware. */ + const uint8_t index; + + /** + * Width of the sub-register field, in bytes, either 2 or 4. */ + const uint8_t width; + + /** + * Length of how many sub-register fields comprise this accessor. Not + * required to stay within the bounds of a single register. */ + const uint16_t length; + +private: + Registers* registers_; }; template -class Accessor : public Accessor_ { +class Accessor : public Accessor_ +{ public: Accessor(Registers* registers, uint8_t register_index, uint8_t array_length = 0, double scale_factor = 1.0) : Accessor_(registers, register_index, sizeof(RegT), array_length), scale_(scale_factor) {} - RegT get(uint8_t field) const { + RegT get(uint8_t field) const + { RegT* raw_ptr = reinterpret_cast(raw()); RegT value; memcpy_network(&value, raw_ptr + field, sizeof(value)); return value; } - double get_scaled(uint16_t field) const { + double get_scaled(uint16_t field) const + { return get(field) * scale_; } - void set(uint8_t field, RegT value) const { + void set(uint8_t field, RegT value) const + { RegT* raw_ptr = reinterpret_cast(raw()); memcpy_network(raw_ptr + field, &value, sizeof(value)); } - void set_scaled(uint16_t field, double value) const { + void set_scaled(uint16_t field, double value) const + { set(field, value / scale_); } @@ -149,22 +154,23 @@ class Accessor : public Accessor_ { const double scale_; }; -class Registers { +class Registers +{ public: Registers() : gyro_raw(this, DREG_GYRO_RAW_XY, 3), accel_raw(this, DREG_ACCEL_RAW_XY, 3), mag_raw(this, DREG_MAG_RAW_XY, 3), gyro(this, DREG_GYRO_PROC_X, 3, 1.0 * TO_RADIANS), - accel(this, DREG_ACCEL_PROC_X, 3, 9.80665), - mag(this, DREG_MAG_PROC_X, 3,1.0), + accel(this, DREG_ACCEL_PROC_X, 3, 9.80665), + mag(this, DREG_MAG_PROC_X, 3, 1.0), euler(this, DREG_EULER_PHI_THETA, 3, 0.0109863 * TO_RADIANS), quat(this, DREG_QUAT_AB, 4, 0.0000335693), temperature(this, DREG_TEMPERATURE, 1), communication(this, CREG_COM_SETTINGS, 1), comrate2(this, CREG_COM_RATES2, 1), comrate4(this, CREG_COM_RATES4, 1), - comrate5(this, CREG_COM_RATES5, 1), + comrate5(this, CREG_COM_RATES5, 1), comrate6(this, CREG_COM_RATES6, 1), misc_config(this, CREG_MISC_SETTINGS, 1), status(this, CREG_COM_RATES6, 1), @@ -178,19 +184,20 @@ class Registers { // Data const Accessor gyro_raw, accel_raw, euler, mag_raw, quat; - + const Accessor gyro, accel, mag, temperature; // Configs const Accessor communication, misc_config, status, comrate2, comrate4, comrate5, comrate6; - const Accessor mag_bias; + const Accessor mag_bias; // Commands const Accessor cmd_zero_gyros, cmd_reset_ekf, cmd_set_mag_ref; - void write_raw(uint8_t register_index, std::string data) { + void write_raw(uint8_t register_index, std::string data) + { if ((register_index - 1) + (data.length()/4 - 1) >= NUM_REGISTERS) { throw std::range_error("Index and length write beyond boundaries of register array."); } @@ -202,6 +209,6 @@ class Registers { friend class Accessor_; }; -} +} // namespace um7 -#endif // INCLUDE_UM7_REGISTERS_H_ +#endif // UM7_REGISTERS_H_ diff --git a/package.xml b/package.xml index 673e032..2e43e50 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + um7 0.0.3 The um7 package provides a C++ implementation of the CH Robotics serial protocol, and a @@ -15,13 +15,14 @@ catkin - serial - roscpp - sensor_msgs + + serial + roscpp + sensor_msgs + message_generation - roslint - serial - roscpp - sensor_msgs - message_runtime + + message_runtime + + roslint diff --git a/src/comms.cpp b/src/comms.cpp index c9134c3..0febb10 100644 --- a/src/comms.cpp +++ b/src/comms.cpp @@ -1,11 +1,11 @@ /** * * \file - * \brief Implementation of Comms class methods to handle reading and + * \brief Implementation of Comms class methods to handle reading and * writing to the UM7 serial interface. * \author Mike Purvis * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * Redistributions of source code must retain the above copyright @@ -16,7 +16,7 @@ * * Neither the name of Clearpath Robotics, Inc. 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 @@ -27,8 +27,8 @@ * 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. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Please send comments, questions, or patches to code@clearpathrobotics.com * */ @@ -115,7 +115,7 @@ int16_t Comms::receive(Registers* registers = NULL) } else { - ROS_DEBUG("Received packet %02x without data.", address); + ROS_INFO("Received packet %02x without data.", address); } // Compare computed checksum with transmitted value. diff --git a/src/main.cpp b/src/main.cpp index 9e2c862..4a63627 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -8,7 +8,7 @@ * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. * \author Alex Brown (adapted to UM7) * \copyright Copyright (c) 2015, Alex Brown. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * Redistributions of source code must retain the above copyright @@ -19,34 +19,31 @@ * * Neither the name of Clearpath Robotics, Inc. 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 CLEARPATH ROBOTICS, INC. OR ALEX BROWN BE LIABLE + * DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. OR ALEX BROWN 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. - * - * Please send comments, questions, or patches to Alex Brown rbirac@cox.net * */ +#include #include "geometry_msgs/Vector3Stamped.h" #include "ros/ros.h" #include "sensor_msgs/Imu.h" -#include "serial/serial.h" // must install serial library from apt-get +#include "serial/serial.h" #include "std_msgs/Float32.h" #include "std_msgs/Header.h" #include "um7/comms.h" #include "um7/registers.h" #include "um7/Reset.h" -#include -float covar[9]; // orientation covariance values const char VERSION[10] = "0.0.2"; // um7_driver version // Don't try to be too clever. Arrival of this message triggers @@ -59,7 +56,7 @@ const uint8_t TRIGGER_PACKET = DREG_EULER_PHI_THETA; */ template void configureVector3(um7::Comms* sensor, const um7::Accessor& reg, - std::string param, std::string human_name) + std::string param, std::string human_name) { if (reg.length != 3) { @@ -103,44 +100,54 @@ void sendCommand(um7::Comms* sensor, const um7::Accessor& reg, std::string * Send configuration messages to the UM7, critically, to turn on the value outputs * which we require, and inject necessary configuration parameters. */ -void configureSensor(um7::Comms* sensor) +void configureSensor(um7::Comms* sensor, ros::NodeHandle *private_nh) { um7::Registers r; - uint32_t comm_reg = (BAUD_115200 << COM_BAUD_START); - r.communication.set(0, comm_reg); - if (!sensor->sendWaitAck(r.comrate2)) - { - throw std::runtime_error("Unable to set CREG_COM_SETTINGS."); - } + uint32_t comm_reg = (BAUD_115200 << COM_BAUD_START); + r.communication.set(0, comm_reg); + if (!sensor->sendWaitAck(r.comrate2)) + { + throw std::runtime_error("Unable to set CREG_COM_SETTINGS."); + } - uint32_t raw_rate = (20 << RATE2_ALL_RAW_START); - r.comrate2.set(0, raw_rate); - if (!sensor->sendWaitAck(r.comrate2)) - { - throw std::runtime_error("Unable to set CREG_COM_RATES2."); - } + // set the broadcast rate of the device + int rate; + private_nh->param("update_rate", rate, 20); + if (rate < 20 || rate > 255) + { + ROS_WARN("Potentially unsupported update rate of %d", rate); + } - uint32_t proc_rate = (20 << RATE4_ALL_PROC_START); - r.comrate4.set(0, proc_rate); - if (!sensor->sendWaitAck(r.comrate4)) - { - throw std::runtime_error("Unable to set CREG_COM_RATES4."); - } + uint32_t rate_bits = static_cast(rate); + ROS_INFO("Setting update rate to %uHz", rate); + uint32_t raw_rate = (rate_bits << RATE2_ALL_RAW_START); + r.comrate2.set(0, raw_rate); + if (!sensor->sendWaitAck(r.comrate2)) + { + throw std::runtime_error("Unable to set CREG_COM_RATES2."); + } - uint32_t misc_rate = (20 << RATE5_EULER_START) | (20 << RATE5_QUAT_START); - r.comrate5.set(0, misc_rate); - if (!sensor->sendWaitAck(r.comrate5)) - { - throw std::runtime_error("Unable to set CREG_COM_RATES5."); - } + uint32_t proc_rate = (rate_bits << RATE4_ALL_PROC_START); + r.comrate4.set(0, proc_rate); + if (!sensor->sendWaitAck(r.comrate4)) + { + throw std::runtime_error("Unable to set CREG_COM_RATES4."); + } - uint32_t health_rate = (5 << RATE6_HEALTH_START); // note: 5 gives 2 hz rate - r.comrate6.set(0, health_rate); - if (!sensor->sendWaitAck(r.comrate6)) - { - throw std::runtime_error("Unable to set CREG_COM_RATES6."); - } + uint32_t misc_rate = (rate_bits << RATE5_EULER_START) | (rate_bits << RATE5_QUAT_START); + r.comrate5.set(0, misc_rate); + if (!sensor->sendWaitAck(r.comrate5)) + { + throw std::runtime_error("Unable to set CREG_COM_RATES5."); + } + + uint32_t health_rate = (5 << RATE6_HEALTH_START); // note: 5 gives 2 hz rate + r.comrate6.set(0, health_rate); + if (!sensor->sendWaitAck(r.comrate6)) + { + throw std::runtime_error("Unable to set CREG_COM_RATES6."); + } // Options available using parameters) @@ -148,7 +155,7 @@ void configureSensor(um7::Comms* sensor) // Optionally disable mag updates in the sensor's EKF. bool mag_updates; - ros::param::param("~mag_updates", mag_updates, true); + private_nh->param("mag_updates", mag_updates, true); if (mag_updates) { misc_config_reg |= MAG_UPDATES_ENABLED; @@ -160,7 +167,7 @@ void configureSensor(um7::Comms* sensor) // Optionally enable quaternion mode . bool quat_mode; - ros::param::param("~quat_mode", quat_mode, true); + private_nh->param("quat_mode", quat_mode, true); if (quat_mode) { misc_config_reg |= QUATERNION_MODE_ENABLED; @@ -178,7 +185,7 @@ void configureSensor(um7::Comms* sensor) // Optionally disable performing a zero gyros command on driver startup. bool zero_gyros; - ros::param::param("~zero_gyros", zero_gyros, true); + private_nh->param("zero_gyros", zero_gyros, true); if (zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes"); } @@ -197,44 +204,50 @@ bool handleResetService(um7::Comms* sensor, * Uses the register accessors to grab data from the IMU, and populate * the ROS messages which are output. */ -void publishMsgs(um7::Registers& r, ros::NodeHandle* n, const std_msgs::Header& header) +void publishMsgs(um7::Registers& r, ros::NodeHandle* imu_nh, sensor_msgs::Imu& imu_msg, bool tf_ned_to_enu) { - static ros::Publisher imu_pub = n->advertise("imu/data", 1, false); - static ros::Publisher mag_pub = n->advertise("imu/mag", 1, false); - static ros::Publisher rpy_pub = n->advertise("imu/rpy", 1, false); - static ros::Publisher temp_pub = n->advertise("imu/temperature", 1, false); + static ros::Publisher imu_pub = imu_nh->advertise("data", 1, false); + static ros::Publisher mag_pub = imu_nh->advertise("mag", 1, false); + static ros::Publisher rpy_pub = imu_nh->advertise("rpy", 1, false); + static ros::Publisher temp_pub = imu_nh->advertise("temperature", 1, false); if (imu_pub.getNumSubscribers() > 0) { - sensor_msgs::Imu imu_msg; - imu_msg.header = header; - - // IMU outputs [w,x,y,z], convert to [x,y,z,w] & transform to ROS axes - imu_msg.orientation.x = r.quat.get_scaled(1); - imu_msg.orientation.y = -r.quat.get_scaled(2); - imu_msg.orientation.z = -r.quat.get_scaled(3); - imu_msg.orientation.w = r.quat.get_scaled(0); - - // Covariance of attitude. set to constant default or parameter values - imu_msg.orientation_covariance[0] = covar[0]; - imu_msg.orientation_covariance[1] = covar[1]; - imu_msg.orientation_covariance[2] = covar[2]; - imu_msg.orientation_covariance[3] = covar[3]; - imu_msg.orientation_covariance[4] = covar[4]; - imu_msg.orientation_covariance[5] = covar[5]; - imu_msg.orientation_covariance[6] = covar[6]; - imu_msg.orientation_covariance[7] = covar[7]; - imu_msg.orientation_covariance[8] = covar[8]; - - // Angular velocity. transform to ROS axes - imu_msg.angular_velocity.x = r.gyro.get_scaled(0); - imu_msg.angular_velocity.y = -r.gyro.get_scaled(1); - imu_msg.angular_velocity.z = -r.gyro.get_scaled(2); - - // Linear accel. transform to ROS axes - imu_msg.linear_acceleration.x = r.accel.get_scaled(0); - imu_msg.linear_acceleration.y = -r.accel.get_scaled(1); - imu_msg.linear_acceleration.z = -r.accel.get_scaled(2); + // body-fixed frame NED to ENU: (x y z)->(x -y -z) or (w x y z)->(x -y -z w) + // world frame NED to ENU: (x y z)->(y x -z) or (w x y z)->(y x -z w) + if (tf_ned_to_enu) + { + // world frame + imu_msg.orientation.w = r.quat.get_scaled(2); + imu_msg.orientation.x = r.quat.get_scaled(1); + imu_msg.orientation.y = -r.quat.get_scaled(3); + imu_msg.orientation.z = r.quat.get_scaled(0); + + // body-fixed frame + imu_msg.angular_velocity.x = r.gyro.get_scaled(0); + imu_msg.angular_velocity.y = -r.gyro.get_scaled(1); + imu_msg.angular_velocity.z = -r.gyro.get_scaled(2); + + // body-fixed frame + imu_msg.linear_acceleration.x = r.accel.get_scaled(0); + imu_msg.linear_acceleration.y = -r.accel.get_scaled(1); + imu_msg.linear_acceleration.z = -r.accel.get_scaled(2); + } + else + { + imu_msg.orientation.w = r.quat.get_scaled(0); + imu_msg.orientation.x = r.quat.get_scaled(1); + imu_msg.orientation.y = r.quat.get_scaled(2); + imu_msg.orientation.z = r.quat.get_scaled(3); + + imu_msg.angular_velocity.x = r.gyro.get_scaled(0); + imu_msg.angular_velocity.y = r.gyro.get_scaled(1); + imu_msg.angular_velocity.z = r.gyro.get_scaled(2); + + imu_msg.linear_acceleration.x = r.accel.get_scaled(0); + imu_msg.linear_acceleration.y = r.accel.get_scaled(1); + imu_msg.linear_acceleration.z = r.accel.get_scaled(2); + } imu_pub.publish(imu_msg); } @@ -243,10 +256,22 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* n, const std_msgs::Header& if (mag_pub.getNumSubscribers() > 0) { geometry_msgs::Vector3Stamped mag_msg; - mag_msg.header = header; - mag_msg.vector.x = r.mag.get_scaled(0); - mag_msg.vector.y = -r.mag.get_scaled(1); - mag_msg.vector.z = -r.mag.get_scaled(2); + mag_msg.header = imu_msg.header; + + if (tf_ned_to_enu) + { + // world frame + mag_msg.vector.x = r.mag.get_scaled(1); + mag_msg.vector.y = r.mag.get_scaled(0); + mag_msg.vector.z = -r.mag.get_scaled(2); + } + else + { + mag_msg.vector.x = r.mag.get_scaled(0); + mag_msg.vector.y = r.mag.get_scaled(1); + mag_msg.vector.z = r.mag.get_scaled(2); + } + mag_pub.publish(mag_msg); } @@ -254,10 +279,22 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* n, const std_msgs::Header& if (rpy_pub.getNumSubscribers() > 0) { geometry_msgs::Vector3Stamped rpy_msg; - rpy_msg.header = header; - rpy_msg.vector.x = r.euler.get_scaled(0); - rpy_msg.vector.y = -r.euler.get_scaled(1); - rpy_msg.vector.z = -r.euler.get_scaled(2); + rpy_msg.header = imu_msg.header; + + if (tf_ned_to_enu) + { + // world frame + rpy_msg.vector.x = r.euler.get_scaled(1); + rpy_msg.vector.y = r.euler.get_scaled(0); + rpy_msg.vector.z = -r.euler.get_scaled(2); + } + else + { + rpy_msg.vector.x = r.euler.get_scaled(0); + rpy_msg.vector.y = r.euler.get_scaled(1); + rpy_msg.vector.z = r.euler.get_scaled(2); + } + rpy_pub.publish(rpy_msg); } @@ -270,7 +307,6 @@ void publishMsgs(um7::Registers& r, ros::NodeHandle* n, const std_msgs::Header& } } - /** * Node entry-point. Handles ROS setup, and serial port connection/reconnection. */ @@ -281,8 +317,10 @@ int main(int argc, char **argv) // Load parameters from private node handle. std::string port; int32_t baud; - ros::param::param("~port", port, "/dev/ttyUSB0"); - ros::param::param("~baud", baud, 115200); + + ros::NodeHandle imu_nh("imu"), private_nh("~"); + private_nh.param("port", port, "/dev/ttyUSB0"); + private_nh.param("baud", baud, 115200); serial::Serial ser; ser.setPort(port); @@ -290,30 +328,42 @@ int main(int argc, char **argv) serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0); ser.setTimeout(to); - ros::NodeHandle n; + sensor_msgs::Imu imu_msg; + double linear_acceleration_stdev, angular_velocity_stdev; + private_nh.param("frame_id", imu_msg.header.frame_id, "imu_link"); + // Defaults obtained experimentally from hardware, no device spec exists + private_nh.param("linear_acceleration_stdev", linear_acceleration_stdev, (4.0 * 1e-3f * 9.80665)); + private_nh.param("angular_velocity_stdev", angular_velocity_stdev, (0.06 * 3.14159 / 180.0)); - std_msgs::Header header; - ros::param::param("~frame_id", header.frame_id, "imu_link"); + double linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev; + double angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev; - // Initialize covariance. The UM7 sensor does not provide covariance values so, - // by default, this driver provides a covariance array of all zeros indicating - // "covariance unknown" as advised in sensor_msgs/Imu.h. - // This param allows the user to specify alternate covariance values if needed. + // From the UM7 datasheet for the dynamic accuracy from the EKF. + double orientation_x_stdev, orientation_y_stdev, orientation_z_stdev; + private_nh.param("orientation_x_stdev", orientation_x_stdev, (3.0 * 3.14159 / 180.0)); + private_nh.param("orientation_y_stdev", orientation_y_stdev, (3.0 * 3.14159 / 180.0)); + private_nh.param("orientation_z_stdev", orientation_z_stdev, (5.0 * 3.14159 / 180.0)); - std::string covariance; - char cov[200]; - char *ptr1; + double orientation_x_covar = orientation_x_stdev * orientation_x_stdev; + double orientation_y_covar = orientation_y_stdev * orientation_y_stdev; + double orientation_z_covar = orientation_z_stdev * orientation_z_stdev; - ros::param::param("~covariance", covariance, "0 0 0 0 0 0 0 0 0"); - snprintf(cov, sizeof(cov), "%s", covariance.c_str()); + // Enable converting from NED to ENU by default + bool tf_ned_to_enu; + private_nh.param("tf_ned_to_enu", tf_ned_to_enu, true); - char* p = strtok_r(cov, " ", &ptr1); // point to first value - for (int iter = 0; iter < 9; iter++) - { - if (p) covar[iter] = atof(p); // covar[] is global var - else covar[iter] = 0.0; - p = strtok_r(NULL, " ", &ptr1); // point to next value (nil if none) - } + // These values do not need to be converted + imu_msg.linear_acceleration_covariance[0] = linear_acceleration_cov; + imu_msg.linear_acceleration_covariance[4] = linear_acceleration_cov; + imu_msg.linear_acceleration_covariance[8] = linear_acceleration_cov; + + imu_msg.angular_velocity_covariance[0] = angular_velocity_cov; + imu_msg.angular_velocity_covariance[4] = angular_velocity_cov; + imu_msg.angular_velocity_covariance[8] = angular_velocity_cov; + + imu_msg.orientation_covariance[0] = orientation_x_covar; + imu_msg.orientation_covariance[4] = orientation_y_covar; + imu_msg.orientation_covariance[8] = orientation_z_covar; // Real Time Loop bool first_failure = true; @@ -323,20 +373,20 @@ int main(int argc, char **argv) { ser.open(); } - catch(const serial::IOException& e) + catch (const serial::IOException& e) { - ROS_DEBUG("um7_driver ver %s unable to connect to port.", VERSION); + ROS_WARN("um7_driver was unable to connect to port %s.", port.c_str()); } if (ser.isOpen()) { - ROS_INFO("um7_driver ver %s connected to serial port.", VERSION); + ROS_INFO("um7_driver successfully connected to serial port %s.", port.c_str()); first_failure = true; try { um7::Comms sensor(&ser); - configureSensor(&sensor); + configureSensor(&sensor, &private_nh); um7::Registers registers; - ros::ServiceServer srv = n.advertiseService( + ros::ServiceServer srv = imu_nh.advertiseService( "reset", boost::bind(handleResetService, &sensor, _1, _2)); while (ros::ok()) @@ -344,8 +394,9 @@ int main(int argc, char **argv) // triggered by arrival of last message packet if (sensor.receive(®isters) == TRIGGER_PACKET) { - header.stamp = ros::Time::now(); - publishMsgs(registers, &n, header); + // Triggered by arrival of final message in group. + imu_msg.header.stamp = ros::Time::now(); + publishMsgs(registers, &imu_nh, imu_msg, tf_ned_to_enu); ros::spinOnce(); } } @@ -361,7 +412,7 @@ int main(int argc, char **argv) else { ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device " - << port << ". Trying again every 1 second."); + << port << ". Trying again every 1 second."); first_failure = false; ros::Duration(1.0).sleep(); } diff --git a/src/registers.cpp b/src/registers.cpp index 0c9cc4e..d9f0783 100644 --- a/src/registers.cpp +++ b/src/registers.cpp @@ -4,7 +4,7 @@ * \brief Stub method from the Accessor class. * \author Mike Purvis * \copyright Copyright (c) 2013, Clearpath Robotics, Inc. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * Redistributions of source code must retain the above copyright @@ -15,7 +15,7 @@ * * Neither the name of Clearpath Robotics, Inc. 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 @@ -26,8 +26,8 @@ * 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. - * - * Please send comments, questions, or patches to code@clearpathrobotics.com + * + * Please send comments, questions, or patches to code@clearpathrobotics.com * */ diff --git a/test/test_comms.cpp b/test/test_comms.cpp index 95321e5..eddd97f 100644 --- a/test/test_comms.cpp +++ b/test/test_comms.cpp @@ -4,41 +4,46 @@ #include #include -class FakeSerial : public ::testing::Test { - protected: - /** - * What's going on here? The posix_openpt() call establishes a pseudo terminal - * and gives us a fd for the other end of it. So we can connect up a Serial - * instance to the pty, and have full control over reading-from and writing-to - * the driver. - */ - virtual void SetUp() { - ASSERT_NE(-1, master_fd = posix_openpt( O_RDWR | O_NOCTTY | O_NDELAY )); - ASSERT_NE(-1, grantpt(master_fd)); - ASSERT_NE(-1, unlockpt(master_fd)); - ASSERT_TRUE((ser_name = ptsname(master_fd)) != NULL); - ser.setPort(ser_name); - ser.open(); - ASSERT_TRUE(ser.isOpen()) << "Couldn't open Serial connection to pseudoterminal."; - } +class FakeSerial : public ::testing::Test +{ +protected: + /** + * What's going on here? The posix_openpt() call establishes a pseudo terminal + * and gives us a fd for the other end of it. So we can connect up a Serial + * instance to the pty, and have full control over reading-from and writing-to + * the driver. + */ + virtual void SetUp() + { + ASSERT_NE(-1, master_fd = posix_openpt( O_RDWR | O_NOCTTY | O_NDELAY )); + ASSERT_NE(-1, grantpt(master_fd)); + ASSERT_NE(-1, unlockpt(master_fd)); + ASSERT_TRUE((ser_name = ptsname(master_fd)) != NULL); + ser.setPort(ser_name); + ser.open(); + ASSERT_TRUE(ser.isOpen()) << "Couldn't open Serial connection to pseudoterminal."; + } - void write_serial(const std::string& msg) { - write(master_fd, msg.c_str(), msg.length()); - } + void write_serial(const std::string& msg) + { + write(master_fd, msg.c_str(), msg.length()); + } - virtual void TearDown() { - ser.close(); - close(master_fd); - } + virtual void TearDown() + { + ser.close(); + close(master_fd); + } - serial::Serial ser; + serial::Serial ser; - private: - int master_fd; - char* ser_name; +private: + int master_fd; + char* ser_name; }; -TEST_F(FakeSerial, basic_message_rx) { +TEST_F(FakeSerial, basic_message_rx) +{ // Send message from device which should write four bytes to the raw magnetometer's first register. std::string msg(um7::Comms::message(DREG_MAG_RAW_XY, std::string("\x1\x2\x3\x4"))); write_serial(msg); @@ -49,7 +54,8 @@ TEST_F(FakeSerial, basic_message_rx) { EXPECT_EQ(0x0102, registers.mag_raw.get(0)); } -TEST_F(FakeSerial, batch_message_rx) { +TEST_F(FakeSerial, batch_message_rx) +{ // Send message from device which should write four bytes to the raw accelerometer's registers. std::string msg(um7::Comms::message(DREG_ACCEL_RAW_XY, std::string("\x5\x6\x7\x8\x9\xa\0\0", 8))); write_serial(msg); @@ -62,7 +68,8 @@ TEST_F(FakeSerial, batch_message_rx) { EXPECT_EQ(0x090a, registers.accel_raw.get(2)); } -TEST_F(FakeSerial, bad_checksum_message_rx) { +TEST_F(FakeSerial, bad_checksum_message_rx) +{ // Generate message, then twiddle final byte. std::string msg(um7::Comms::message(DREG_MAG_RAW_XY, std::string("\x1\x2\x3\x4"))); msg[msg.length() - 1]++; @@ -73,7 +80,8 @@ TEST_F(FakeSerial, bad_checksum_message_rx) { EXPECT_EQ(-1, sensor.receive(®isters)) << "Didn't properly ignore bad checksum message."; } -TEST_F(FakeSerial, garbage_bytes_preceeding_message_rx) { +TEST_F(FakeSerial, garbage_bytes_preceeding_message_rx) +{ // Generate message, then prepend junk. std::string msg(um7::Comms::message(CONFIG_REG_START_ADDRESS, std::string())); msg = "ssssssnsnsns" + msg; @@ -83,14 +91,16 @@ TEST_F(FakeSerial, garbage_bytes_preceeding_message_rx) { EXPECT_EQ(CONFIG_REG_START_ADDRESS, sensor.receive(NULL)) << "Didn't handle garbage prepended to message."; } -TEST_F(FakeSerial, timeout_message_rx) { +TEST_F(FakeSerial, timeout_message_rx) +{ std::string msg("snp\x12\x45"); write_serial(msg); um7::Comms sensor(&ser); EXPECT_EQ(-1, sensor.receive(NULL)) << "Didn't properly time out in the face of a partial message."; } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } diff --git a/test/test_registers.cpp b/test/test_registers.cpp index bed0a8d..ac58ef1 100644 --- a/test/test_registers.cpp +++ b/test/test_registers.cpp @@ -85,7 +85,8 @@ TEST(Accessor, set_float) EXPECT_FLOAT_EQ(0.555, check); } -int main(int argc, char **argv){ +int main(int argc, char **argv) +{ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); }