diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e8427d8 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 2.8.3) +project(um7) + +find_package(catkin REQUIRED COMPONENTS roscpp serial roslint sensor_msgs + message_generation) + +add_service_files( + FILES + Reset.srv +) + +generate_messages() + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS message_runtime +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp executable +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 ## +############# + +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 ## +############# + +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) + 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() + + diff --git a/README.md b/README.md new file mode 100644 index 0000000..db2d5c7 --- /dev/null +++ b/README.md @@ -0,0 +1,6 @@ +ros-drivers-um7 +=============== + +ROS driver for the CH Robotics UM7 inertial measurement device. +Supports standard data and mag topics as well as providing temperature and rpy outputs. + See the ROS wiki for details: (soon to come) diff --git a/include/um7/comms.h b/include/um7/comms.h new file mode 100644 index 0000000..94349f9 --- /dev/null +++ b/include/um7/comms.h @@ -0,0 +1,87 @@ +/** + * + * \file + * \brief Comms class definition. Does not manage the serial connection + * itself, but takes care of reading and writing to UM7. + * \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 + * notice, this list of conditions and the following disclaimer. + * * 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. + * * 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. 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 code@clearpathrobotics.com + * + */ + +#ifndef INCLUDE_UM7_COMMS_H_ +#define INCLUDE_UM7_COMMS_H_ + +#include +#include + +namespace serial { + class Serial; +} + +namespace um7 { + +class SerialTimeout : public std::exception {}; + +class BadChecksum : public std::exception {}; + +class Registers; +class Accessor_; + +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); + + void send(const Accessor_& a) const; + + 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 std::string checksum(const std::string& s); + + static std::string message(uint8_t address, std::string data); + + private: + bool first_spin_; + serial::Serial* serial_; +}; +} + +#endif // INCLUDE_UM7_COMMS_H_ + diff --git a/include/um7/firmware_registers.h b/include/um7/firmware_registers.h new file mode 100644 index 0000000..09cf3c6 --- /dev/null +++ b/include/um7/firmware_registers.h @@ -0,0 +1,293 @@ + /** + * \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 + */ + + +// Define the firmware revision +#define UM6_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 +// 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 +// software running on the sensor knows exactly where to find it - it needn't know what the data is. The software communicatin +// 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 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 + +// 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 + +// 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_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) + +// 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 + +// 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 + +// Bit definitions for filter settings register. +#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) + +// 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 + +// 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) + +// 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 + +// 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) + +// 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 + +// 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 + +// Start address for writing configuration to FLASH +#define FLASH_START_ADDRESS (uint32_t)0x0800F000 +// Start address for factory FLASH configuration +#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 GYRO_ZERO_SAMPLE_SIZE 500 + +typedef struct __CHR_config +{ + 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]; + }; +} CHR_data; + + diff --git a/include/um7/registers.h b/include/um7/registers.h new file mode 100644 index 0000000..29da179 --- /dev/null +++ b/include/um7/registers.h @@ -0,0 +1,207 @@ +/** + * + * \file + * \brief Provides the Registers class, which initializes with a suite + * of accessors suitable for reading and writing the UM7 registers, + * including byte-order conversion and scaling handled. + * \author Mike Purvis wrote original code for UM6 + * \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 + * notice, this list of conditions and the following disclaimer. + * * 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. + * * 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. 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 + * + */ + +#ifndef INCLUDE_UM7_REGISTERS_H_ +#define INCLUDE_UM7_REGISTERS_H_ + +#if __APPLE__ +#include +#else +#include +#endif + +#include +#include +#include +#include + +#include +#include + +#include "um7/firmware_registers.h" + +#define TO_RADIANS (M_PI / 180.0) +#define TO_DEGREES (180.0 / M_PI) + +// This excludes the command registers, which are always sent +// and received with no data. +#define NUM_REGISTERS (DATA_REG_START_ADDRESS + DATA_ARRAY_SIZE) + + +namespace um7 { + +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); + for (uint8_t i = 0; i < count; i++) { + d[i] = s[count - (i+1)]; + } +#else + // Copy bytes without reversing. + #warning Big-endian implementation is untested. + memcpy(dest, src, count); +#endif +} + +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 + * 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_; +}; + +template +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* raw_ptr = reinterpret_cast(raw()); + RegT value; + memcpy_network(&value, raw_ptr + field, sizeof(value)); + return value; + } + + double get_scaled(uint16_t field) const { + return get(field) * scale_; + } + + 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 { + set(field, value / scale_); + } + + private: + const double scale_; +}; + +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), + 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), + comrate6(this, CREG_COM_RATES6, 1), + misc_config(this, CREG_MISC_SETTINGS, 1), + status(this, CREG_COM_RATES6, 1), + mag_bias(this, CREG_MAG_BIAS_X, 3), + cmd_zero_gyros(this, CHR_ZERO_GYROS), + cmd_reset_ekf(this, CHR_RESET_EKF), + cmd_set_mag_ref(this, CHR_SET_MAG_REFERENCE) + { + memset(raw_, 0, sizeof(raw_)); + } + + // 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; + + // Commands + const Accessor cmd_zero_gyros, cmd_reset_ekf, cmd_set_mag_ref; + + 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."); + } + memcpy(&raw_[register_index], data.c_str(), data.length()); + } + + private: + uint32_t raw_[NUM_REGISTERS]; + + friend class Accessor_; +}; +} + +#endif // INCLUDE_UM7_REGISTERS_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..330e04d --- /dev/null +++ b/package.xml @@ -0,0 +1,27 @@ + + + um7 + 0.0.2 + The um7 package provides a C++ implementation of the CH Robotics serial protocol, and a + corresponding ROS node for publishing standard ROS orientation topics from a UM7. + + + Alex Brown + Mike Purvis + Alex Brown + + BSD + + + + catkin + serial + roscpp + sensor_msgs + message_generation + roslint + serial + roscpp + sensor_msgs + message_runtime + diff --git a/src/comms.cpp b/src/comms.cpp new file mode 100644 index 0000000..2357878 --- /dev/null +++ b/src/comms.cpp @@ -0,0 +1,222 @@ +/** + * + * \file + * \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 + * notice, this list of conditions and the following disclaimer. + * * 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. + * * 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. 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 code@clearpathrobotics.com + * + */ + +#include "um7/comms.h" + +#include +#include +#include +#include + +#include "ros/console.h" +#include "serial/serial.h" +#include "um7/registers.h" + +namespace um7 +{ + +const uint8_t Comms::PACKET_HAS_DATA = 1 << 7; +const uint8_t Comms::PACKET_IS_BATCH = 1 << 6; +const uint8_t Comms::PACKET_BATCH_LENGTH_MASK = 0x0F; +const uint8_t Comms::PACKET_BATCH_LENGTH_OFFSET = 2; + +int16_t Comms::receive(Registers* registers = NULL) +{ + // Search the serial stream for a start-of-packet sequence. + try + { + size_t available = serial_->available(); + if (available > 255) + { + ROS_WARN_STREAM("Serial read buffer is " << available << ", now flushing in an attempt to catch up."); + serial_->flushInput(); + } + + // Optimistically assume that the next five bytes on the wire are a packet header. + uint8_t header_bytes[5]; + serial_->read(header_bytes, 5); + + uint8_t type, address; + if (memcmp(header_bytes, "snp", 3) == 0) + { + // Optimism win. + type = header_bytes[3]; + address = header_bytes[4]; + } + else + { + // Optimism fail. Search the serial stream for a header. + std::string snp; + serial_->readline(snp, 96, "snp"); + if (!boost::algorithm::ends_with(snp, "snp")) throw SerialTimeout(); + if (snp.length() > 3) + { + ROS_WARN_STREAM_COND(!first_spin_, + "Discarded " << 5 + snp.length() - 3 << " junk byte(s) preceeding packet."); + } + if (serial_->read(&type, 1) != 1) throw SerialTimeout(); + if (serial_->read(&address, 1) != 1) throw SerialTimeout(); + } + + first_spin_ = false; + + uint16_t checksum_calculated = 's' + 'n' + 'p' + type + address; + std::string data; + if (type & PACKET_HAS_DATA) + { + uint8_t data_length = 1; + if (type & PACKET_IS_BATCH) + { + data_length = (type >> PACKET_BATCH_LENGTH_OFFSET) & PACKET_BATCH_LENGTH_MASK; + ROS_DEBUG("Received packet %02x with batched (%d) data.", address, data_length); + } + else + { + ROS_DEBUG("Received packet %02x with non-batched data.", address); + } + + // Read data bytes initially into a buffer so that we can compute the checksum. + if (serial_->read(data, data_length * 4) != data_length * 4) throw SerialTimeout(); + BOOST_FOREACH(uint8_t ch, data) + { + checksum_calculated += ch; + } + } + else + { + ROS_DEBUG("Received packet %02x without data.", address); + } + + // Compare computed checksum with transmitted value. + uint16_t checksum_transmitted; + if (serial_->read(reinterpret_cast(&checksum_transmitted), 2) != 2) + { + throw SerialTimeout(); + } + checksum_transmitted = ntohs(checksum_transmitted); + if (checksum_transmitted != checksum_calculated) + { + throw BadChecksum(); + } + + // Copy data from checksum buffer into registers, if specified. + // Note that byte-order correction (as necessary) happens at access-time. + if ((data.length() > 0) && registers) + { + registers->write_raw(address, data); + } + + // Successful packet read, return address byte. + return address; + } + catch(const SerialTimeout& e) + { + ROS_WARN("Timed out waiting for packet from device."); + } + catch(const BadChecksum& e) + { + ROS_WARN("Discarding packet due to bad checksum."); + } + return -1; +} + +std::string Comms::checksum(const std::string& s) +{ + uint16_t checksum = 0; + BOOST_FOREACH(uint8_t ch, s) + { + checksum += ch; + } + checksum = htons(checksum); + ROS_DEBUG("Computed checksum on string of length %zd as %04x.", s.length(), checksum); + std::string out(2, 0); + memcpy(&out[0], &checksum, 2); + return out; +} + +std::string Comms::message(uint8_t address, std::string data) +{ + uint8_t type = 0; + if (data.length() > 0) + { + type |= PACKET_HAS_DATA; + } + if (data.length() > 4) + { + type |= PACKET_IS_BATCH; + type |= (data.length() / 4) << PACKET_BATCH_LENGTH_OFFSET; + } + + std::stringstream ss(std::stringstream::out | std::stringstream::binary); + ss << "snp" << type << address << data; + std::string output = ss.str(); + std::string c = checksum(output); + ss << c; + output = ss.str(); + ROS_DEBUG("Generated message %02x of overall length %zd.", address, output.length()); + return output; +} + +void Comms::send(const Accessor_& r) const +{ + uint8_t address = r.index; + std::string data(reinterpret_cast(r.raw()), r.length * 4); + serial_->write(message(r.index, data)); +} + +bool Comms::sendWaitAck(const Accessor_& r) +{ + const uint8_t tries = 5; + for (uint8_t t = 0; t < tries; t++) + { + send(r); + const uint8_t listens = 20; + for (uint8_t i = 0; i < listens; i++) + { + int16_t received = receive(); + if (received == r.index) + { + ROS_DEBUG("Message %02x ack received.", received); + return true; + } + else if (received == -1) + { + ROS_DEBUG("Serial read timed out waiting for ack. Attempting to retransmit."); + break; + } + } + } + return false; +} +} // namespace um7 diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..8038736 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,372 @@ +/** + * + * \file + * \brief Main entry point for UM7 driver. Handles serial connection + * details, as well as all ROS message stuffing, parameters, + * topics, etc. + * \author Mike Purvis (original code for UM6) + * \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 + * notice, this list of conditions and the following disclaimer. + * * 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. + * * 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 + * 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 "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 "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 +// us to publish everything we have. +const uint8_t TRIGGER_PACKET = DREG_EULER_PHI_THETA; + +/** + * Function generalizes the process of writing an XYZ vector into consecutive + * fields in UM7 registers. + */ +template +void configureVector3(um7::Comms* sensor, const um7::Accessor& reg, + std::string param, std::string human_name) +{ + if (reg.length != 3) + { + throw std::logic_error("configureVector3 may only be used with 3-field registers!"); + } + + if (ros::param::has(param)) + { + double x, y, z; + ros::param::get(param + "/x", x); + ros::param::get(param + "/y", y); + ros::param::get(param + "/z", z); + ROS_INFO_STREAM("Configuring " << human_name << " to (" + << x << ", " << y << ", " << z << ")"); + reg.set_scaled(0, x); + reg.set_scaled(1, y); + reg.set_scaled(2, z); + if (sensor->sendWaitAck(reg)) + { + throw std::runtime_error("Unable to configure vector."); + } + } +} + +/** + * Function generalizes the process of commanding the UM7 via one of its command + * registers. + */ +template +void sendCommand(um7::Comms* sensor, const um7::Accessor& reg, std::string human_name) +{ + ROS_INFO_STREAM("Sending command: " << human_name); + if (!sensor->sendWaitAck(reg)) + { + throw std::runtime_error("Command to device failed."); + } +} + + +/** + * 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) +{ + 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 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."); + } + + 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 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 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) + uint32_t misc_config_reg = 0; // initialize all options off + + // Optionally disable mag updates in the sensor's EKF. + bool mag_updates; + ros::param::param("~mag_updates", mag_updates, true); + if (mag_updates) + { + misc_config_reg |= MAG_UPDATES_ENABLED; + } + else + { + ROS_WARN("Excluding magnetometer updates from EKF."); + } + + // Optionally enable quaternion mode . + bool quat_mode; + ros::param::param("~quat_mode", quat_mode, true); + if (quat_mode) + { + misc_config_reg |= QUATERNION_MODE_ENABLED; + } + else + { + ROS_WARN("Excluding quaternion mode."); + } + + r.misc_config.set(0, misc_config_reg); + if (!sensor->sendWaitAck(r.misc_config)) + { + throw std::runtime_error("Unable to set CREG_MISC_SETTINGS."); + } + + // Optionally disable performing a zero gyros command on driver startup. + bool zero_gyros; + ros::param::param("~zero_gyros", zero_gyros, true); + if (zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes"); +} + + +bool handleResetService(um7::Comms* sensor, + const um7::Reset::Request& req, const um7::Reset::Response& resp) +{ + um7::Registers r; + if (req.zero_gyros) sendCommand(sensor, r.cmd_zero_gyros, "zero gyroscopes"); + if (req.reset_ekf) sendCommand(sensor, r.cmd_reset_ekf, "reset EKF"); + if (req.set_mag_ref) sendCommand(sensor, r.cmd_set_mag_ref, "set magnetometer reference"); + return true; +} + +/** + * 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) +{ + 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); + + 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); + + imu_pub.publish(imu_msg); + } + + // Magnetometer. transform to ROS axes + 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_pub.publish(mag_msg); + } + + // Euler attitudes. transform to ROS axes + 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_pub.publish(rpy_msg); + } + + // Temperature + if (temp_pub.getNumSubscribers() > 0) + { + std_msgs::Float32 temp_msg; + temp_msg.data = r.temperature.get_scaled(0); + temp_pub.publish(temp_msg); + } +} + + +/** + * Node entry-point. Handles ROS setup, and serial port connection/reconnection. + */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "um7_driver"); + + // 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); + + serial::Serial ser; + ser.setPort(port); + ser.setBaudrate(baud); + serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0); + ser.setTimeout(to); + + ros::NodeHandle n; + + std_msgs::Header header; + ros::param::param("~frame_id", header.frame_id, "imu_link"); + + // 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. + + std::string covariance; + char cov[200]; + char *ptr1; + + ros::param::param("~covariance", covariance, "0 0 0 0 0 0 0 0 0"); + snprintf(cov, sizeof(cov), "%s", covariance.c_str()); +ROS_INFO("cov %s", cov); + + char* p = strtok_r(cov, " ", &ptr1); // point to first value +ROS_INFO("p1 = %p %s",p,p); + for (int iter = 0; iter < 9; iter++) + { +ROS_INFO("p = %p %s",p,p); + 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) + } + + // Real Time Loop + bool first_failure = true; + while (ros::ok()) + { + try + { + ser.open(); + } + catch(const serial::IOException& e) + { + ROS_DEBUG("um7_driver ver %s unable to connect to port.", VERSION); + } + if (ser.isOpen()) + { + ROS_INFO("um7_driver ver %s connected to serial port.", VERSION); + first_failure = true; + try + { + um7::Comms sensor(&ser); + configureSensor(&sensor); + um7::Registers registers; + ros::ServiceServer srv = n.advertiseService( + "reset", boost::bind(handleResetService, &sensor, _1, _2)); + + while (ros::ok()) + { + // triggered by arrival of last message packet + if (sensor.receive(®isters) == TRIGGER_PACKET) + { + header.stamp = ros::Time::now(); + publishMsgs(registers, &n, header); + ros::spinOnce(); + } + } + } + catch(const std::exception& e) + { + if (ser.isOpen()) ser.close(); + ROS_ERROR_STREAM(e.what()); + ROS_INFO("Attempting reconnection after error."); + ros::Duration(1.0).sleep(); + } + } + else + { + ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device " + << port << ". Trying again every 1 second."); + first_failure = false; + ros::Duration(1.0).sleep(); + } + } +} diff --git a/src/registers.cpp b/src/registers.cpp new file mode 100644 index 0000000..0c9cc4e --- /dev/null +++ b/src/registers.cpp @@ -0,0 +1,49 @@ +/** + * + * \file + * \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 + * notice, this list of conditions and the following disclaimer. + * * 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. + * * 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. 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 code@clearpathrobotics.com + * + */ + +#include "um7/registers.h" + +namespace um7 +{ + +void* Accessor_::raw() const +{ + /** + * This is ridiculous to have a whole source file for this tiny implementation, + * but it's necessary to resolve the otherwise circular dependency between the + * Registers and Accessor classes, when Registers contains Accessor instances + * and Accessor is a template class. + */ + return ®isters_->raw_[index]; +} +} // namespace um7 diff --git a/srv/Reset.srv b/srv/Reset.srv new file mode 100644 index 0000000..7d51bbd --- /dev/null +++ b/srv/Reset.srv @@ -0,0 +1,4 @@ +bool zero_gyros +bool reset_ekf +bool set_mag_ref +---