diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/drv_accel.h | 6 | ||||
| -rw-r--r-- | src/drivers/drv_airspeed.h | 5 | ||||
| -rw-r--r-- | src/drivers/drv_baro.h | 4 | ||||
| -rw-r--r-- | src/drivers/drv_gps.h | 4 | ||||
| -rw-r--r-- | src/drivers/drv_gyro.h | 4 | ||||
| -rw-r--r-- | src/drivers/drv_rc_input.h | 3 | ||||
| -rw-r--r-- | src/drivers/drv_sensor.h | 4 | ||||
| -rw-r--r-- | src/drivers/drv_tone_alarm.h | 4 | ||||
| -rw-r--r-- | src/drivers/ets_airspeed/ets_airspeed.cpp | 3 | ||||
| -rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 32 | ||||
| -rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 3 | ||||
| -rw-r--r-- | src/drivers/px4io/px4io_serial.cpp | 28 | ||||
| -rw-r--r-- | src/drivers/rgbled/rgbled.cpp | 27 | ||||
| -rw-r--r-- | src/drivers/roboclaw/RoboClaw.cpp | 328 | ||||
| -rw-r--r-- | src/drivers/roboclaw/RoboClaw.hpp | 192 | ||||
| -rw-r--r-- | src/drivers/roboclaw/module.mk | 41 | ||||
| -rw-r--r-- | src/drivers/roboclaw/roboclaw_main.cpp | 195 |
17 files changed, 851 insertions, 32 deletions
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e7349..7d93ed938 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Accelerometer driver interface. + * @file drv_accel.h + * + * Accelerometer driver interface. */ #ifndef _DRV_ACCEL_H @@ -66,7 +68,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 7bb9ee2af..78f31495d 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -32,7 +32,10 @@ ****************************************************************************/ /** - * @file Airspeed driver interface. + * @file drv_airspeed.h + * + * Airspeed driver interface. + * * @author Simon Wilks */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index aa251889f..e2f0137ae 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Barometric pressure sensor driver interface. + * @file drv_baro.h + * + * Barometric pressure sensor driver interface. */ #ifndef _DRV_BARO_H diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 398cf4870..06e3535b3 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file GPS driver interface. + * @file drv_gps.h + * + * GPS driver interface. */ #ifndef _DRV_GPS_H diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index fefcae50b..2ae8c7058 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Gyroscope driver interface. + * @file drv_gyro.h + * + * Gyroscope driver interface. */ #ifndef _DRV_GYRO_H diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 4decc324e..78ffad9d7 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file R/C input interface. + * @file drv_rc_input.h * + * R/C input interface. */ #ifndef _DRV_RC_INPUT_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 3a89cab9d..8e8b2c1b9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Common sensor API and ioctl definitions. + * @file drv_sensor.h + * + * Common sensor API and ioctl definitions. */ #ifndef _DRV_SENSOR_H diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 2fab37dd2..cb5fd53a5 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -31,7 +31,9 @@ * ****************************************************************************/ -/* +/** + * @file drv_tone_alarm.h + * * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 084671ae2..9d8ad084e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,11 +132,8 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 8bed8a8df..60601e22c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -241,11 +241,18 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; + perf_counter_t _reg7_resets; + perf_counter_t _reg1_resets; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg7_expected; + uint8_t _reg1_expected; + /** * Start automatic measurement. */ @@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), + _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), + _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ) + _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _reg1_expected(0), + _reg7_expected(0) { // enable debug() calls _debug_enabled = true; @@ -526,10 +537,12 @@ void LSM303D::reset() { /* enable accel*/ - write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + write_reg(ADDR_CTRL_REG1, _reg1_expected); /* enable mag */ - write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + _reg7_expected = REG7_CONT_MODE_M; + write_reg(ADDR_CTRL_REG7, _reg7_expected); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); @@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg) void LSM303D::measure() { + if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { + perf_count(_reg1_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1282,6 +1302,12 @@ LSM303D::measure() void LSM303D::mag_measure() { + if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { + perf_count(_reg7_resets); + reset(); + return; + } + /* status register and data as read back from the device */ #pragma pack(push, 1) struct { diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index ee45d46ac..b3003fc1b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,11 +138,8 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - return ret; } - ret = OK; - return ret; } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 236cb918d..43318ca84 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -48,6 +48,7 @@ #include <time.h> #include <errno.h> #include <string.h> +#include <stdio.h> #include <arch/board/board.h> @@ -262,7 +263,8 @@ PX4IO_serial::init() up_enable_irq(PX4IO_SERIAL_VECTOR); /* enable UART in DMA mode, enable error and line idle interrupts */ - /* rCR3 = USART_CR3_EIE;*/ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; /* create semaphores */ @@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete() DMA_SCR_PBURST_SINGLE | DMA_SCR_MBURST_SINGLE); stm32_dmastart(_tx_dma, nullptr, nullptr, false); + //rCR1 &= ~USART_CR1_TE; + //rCR1 |= USART_CR1_TE; rCR3 |= USART_CR3_DMAT; perf_end(_pc_dmasetup); - /* compute the deadline for a 5ms timeout */ + /* compute the deadline for a 10ms timeout */ struct timespec abstime; clock_gettime(CLOCK_REALTIME, &abstime); -#if 0 - abstime.tv_sec++; /* long timeout for testing */ -#else - abstime.tv_nsec += 10000000; /* 0ms timeout */ - if (abstime.tv_nsec > 1000000000) { + abstime.tv_nsec += 10*1000*1000; + if (abstime.tv_nsec >= 1000*1000*1000) { abstime.tv_sec++; - abstime.tv_nsec -= 1000000000; + abstime.tv_nsec -= 1000*1000*1000; } -#endif /* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */ int ret; @@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt() */ if (_rx_dma_status == _dma_status_waiting) { _abort_dma(); - perf_count(_pc_uerrs); + perf_count(_pc_uerrs); /* complete DMA as though in error */ _do_rx_dma_callback(DMA_STATUS_TEIF); @@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt() unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); + + /* stop the receive DMA */ + stm32_dmastop(_rx_dma); + + /* complete the short reception */ + _do_rx_dma_callback(DMA_STATUS_TEIF); return; } @@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma() stm32_dmastop(_rx_dma); } -#endif /* PX4IO_SERIAL_BASE */
\ No newline at end of file +#endif /* PX4IO_SERIAL_BASE */ diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index ea87b37d9..727c86e02 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -172,7 +172,20 @@ RGBLED::probe() bool on, powersave; uint8_t r, g, b; - ret = get(on, powersave, r, g, b); + /** + this may look strange, but is needed. There is a serial + EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to + a bunch of I2C addresses, including the 0x55 used by this + LED device. So we need to do enough operations to be sure + we are talking to the right device. These 3 operations seem + to be enough, as the 3rd one consistently fails if no + RGBLED is on the bus. + */ + if ((ret=get(on, powersave, r, g, b)) != OK || + (ret=send_led_enable(false) != OK) || + (ret=send_led_enable(false) != OK)) { + return ret; + } return ret; } @@ -561,7 +574,7 @@ rgbled_main(int argc, char *argv[]) int ch; /* jump over start/off/etc and look at options first */ - while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) { + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': rgbledadr = strtol(optarg, NULL, 0); @@ -577,7 +590,12 @@ rgbled_main(int argc, char *argv[]) } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; @@ -598,6 +616,9 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } i2cdevice = PX4_I2C_BUS_LED; } } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000..d65a9be36 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include <poll.h> +#include <stdio.h> +#include <math.h> +#include <string.h> +#include <fcntl.h> +#include <termios.h> + +#include <systemlib/err.h> +#include <arch/board/board.h> +#include <mavlink/mavlink_log.h> + +#include <controllib/uorb/UOrbPublication.hpp> +#include <uORB/topics/debug_key_value.h> +#include <drivers/drv_hrt.h> + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): + _address(address), + _pulsesPerRev(pulsesPerRev), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor1Overflow(0), + _motor2Position(0), + _motor2Speed(0), + _motor2Overflow(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; + struct termios uart_config; + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i<nread;i++) { + //printf("%d\t", rbuf[i]); + //} + //printf("\n"); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + uint8_t checksum = rbuf[5]; + uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & + checksum_mask; + // check if checksum is valid + if (checksum != checksum_computed) { + printf("checksum failed: expected %d got %d\n", + checksum, checksum_computed); + return -1; + } + int overFlow = 0; + + if (status & STATUS_REVERSE) { + //printf("roboclaw: reverse\n"); + } + + if (status & STATUS_UNDERFLOW) { + //printf("roboclaw: underflow\n"); + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + //printf("roboclaw: overflow\n"); + overFlow = +1; + } + + static int64_t overflowAmount = 0x100000000LL; + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + _motor1Position = float(int64_t(count) + + _motor1Overflow*overflowAmount)/_pulsesPerRev; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + _motor2Position = float(int64_t(count) + + _motor2Overflow*overflowAmount)/_pulsesPerRev; + } + return 0; +} + +void RoboClaw::printStatus(char *string, size_t n) +{ + snprintf(string, n, + "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } + return -1; +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } + return -1; +} + +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + +int RoboClaw::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } + return 0; +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + //printf("sum\n"); + for (size_t i=0;i<n;i++) { + sum += buf[i]; + //printf("%d\t", buf[i]); + } + //printf("total sum %d\n", sum); + return sum; +} + +int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, + size_t n_data, uint16_t & prev_sum) +{ + tcflush(_uart, TCIOFLUSH); // flush buffers + uint8_t buf[n_data + 3]; + buf[0] = _address; + buf[1] = cmd; + for (size_t i=0;i<n_data;i++) { + buf[i+2] = data[n_data - i - 1]; // MSB + } + uint16_t sum = _sumBytes(buf, n_data + 2); + prev_sum += sum; + buf[n_data + 2] = sum & checksum_mask; + //printf("\nmessage:\n"); + //for (size_t i=0;i<n_data+3;i++) { + //printf("%d\t", buf[i]); + //} + //printf("\n"); + return write(_uart, buf, n_data + 3); +} + +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev) +{ + printf("roboclaw test: starting\n"); + + // setup + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3); + char buf[200]; + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3); + roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3); + for (int i=0; i<10; i++) { + usleep(100000); + roboclaw.readEncoder(RoboClaw::MOTOR_1); + roboclaw.readEncoder(RoboClaw::MOTOR_2); + roboclaw.printStatus(buf,200); + printf("%s", buf); + } + + printf("Test complete\n"); + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp new file mode 100644 index 000000000..e9f35cf95 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file RoboClas.hpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#pragma once + +#include <poll.h> +#include <stdio.h> +#include <controllib/uorb/UOrbSubscription.hpp> +#include <uORB/topics/actuator_controls.h> +#include <drivers/device/i2c.h> + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + * @param pulsesPerRev # of encoder + * pulses per revolution of wheel + */ + RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + int update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void printStatus(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + uint16_t _pulsesPerRev; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription<actuator_controls_s> _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address, + uint16_t pulsesPerRev); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 000000000..1abecf198 --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000..44ed03254 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,195 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + +/** + * @ file roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <math.h> + +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> + +#include <arch/board/board.h> +#include "RoboClaw.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + const char * deviceName = "/dev/ttyS2"; + uint8_t address = 128; + uint16_t pulsesPerRev = 1200; + + if (argc == 2) { + printf("testing with default settings\n"); + } else if (argc != 4) { + printf("usage: roboclaw test device address pulses_per_rev\n"); + exit(-1); + } else { + deviceName = argv[2]; + address = strtoul(argv[3], nullptr, 0); + pulsesPerRev = strtoul(argv[4], nullptr, 0); + } + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + roboclawTest(deviceName, address, pulsesPerRev); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0); + + printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n", + deviceName, address, pulsesPerRev); + + // start + RoboClaw roboclaw(deviceName, address, pulsesPerRev); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 |