From 5aaf1b100b779f4d7cc0de0aa74ccc55a3c82006 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 19 Nov 2019 20:22:50 -0500 Subject: [PATCH] delete unused md25 driver --- boards/airmind/mindpx-v2/default.cmake | 1 - boards/auav/x21/default.cmake | 1 - boards/av/x-v1/default.cmake | 1 - boards/holybro/durandal-v1/default.cmake | 1 - boards/holybro/durandal-v1/stackcheck.cmake | 1 - boards/modalai/fc-v1/default.cmake | 1 - boards/mro/ctrl-zero-f7/default.cmake | 1 - boards/nxp/fmuk66-v3/default.cmake | 1 - boards/px4/fmu-v3/default.cmake | 1 - boards/px4/fmu-v3/rtps.cmake | 1 - boards/px4/fmu-v3/stackcheck.cmake | 1 - boards/px4/fmu-v4/default.cmake | 1 - boards/px4/fmu-v4/stackcheck.cmake | 1 - boards/px4/fmu-v4pro/default.cmake | 1 - boards/px4/fmu-v4pro/rtps.cmake | 1 - boards/px4/fmu-v5/critmonitor.cmake | 1 - boards/px4/fmu-v5/default.cmake | 1 - boards/px4/fmu-v5/irqmonitor.cmake | 1 - boards/px4/fmu-v5/rover.cmake | 1 - boards/px4/fmu-v5/rtps.cmake | 1 - boards/px4/fmu-v5/stackcheck.cmake | 1 - boards/px4/fmu-v5x/default.cmake | 1 - src/drivers/md25/CMakeLists.txt | 42 -- src/drivers/md25/md25.cpp | 631 -------------------- src/drivers/md25/md25.hpp | 309 ---------- src/drivers/md25/md25_main.cpp | 311 ---------- 26 files changed, 1315 deletions(-) delete mode 100644 src/drivers/md25/CMakeLists.txt delete mode 100644 src/drivers/md25/md25.cpp delete mode 100644 src/drivers/md25/md25.hpp delete mode 100644 src/drivers/md25/md25_main.cpp diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 4d71be69cf..8240cf076f 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -34,7 +34,6 @@ px4_add_board( lights/rgbled #lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 5f649ea677..660d883360 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -34,7 +34,6 @@ px4_add_board( #lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 8a13e1fa15..9ea3482e1f 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -35,7 +35,6 @@ px4_add_board( #lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index ba95dfd55b..333b420d8d 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 3a8fd34f20..847935cb40 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -40,7 +40,6 @@ px4_add_board( #lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 #mkblctrl #optical_flow # all available optical flow drivers #osd diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 0b668602bd..73529136ab 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -32,7 +32,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers #osd diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 5a1ad7ab30..6ff20a1166 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers #osd diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index a6dcce0e83..53ed59b14d 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 0c92b534e8..c418f1dc81 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -43,7 +43,6 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 0398236cc4..99f31ea36e 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -43,7 +43,6 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 582920b5bb..c4e5c211c5 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index d37503926e..4c689ac502 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 161391bd43..6f92c6962e 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 5926e7e682..1023760e65 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -36,7 +36,6 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index b79b5b3657..3f2b0dd575 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -36,7 +36,6 @@ px4_add_board( lights/rgbled_ncp5623c #lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index e6ebf414fb..0aa1e7f99f 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers pca9685 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 6047637844..4223a43536 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -38,7 +38,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index e26ccbc641..a8789d4172 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers pca9685 diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 98188c2fcd..7a59033412 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -31,7 +31,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers pca9685 diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index f5fd878776..3d07ee3571 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -38,7 +38,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 3f97b70a52..de3a4967a0 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -39,7 +39,6 @@ px4_add_board( #lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #md25 #mkblctrl optical_flow # all available optical flow drivers #pca9685 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index dd06a1060b..76766f234f 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #md25 mkblctrl optical_flow # all available optical flow drivers #osd diff --git a/src/drivers/md25/CMakeLists.txt b/src/drivers/md25/CMakeLists.txt deleted file mode 100644 index b58ae21708..0000000000 --- a/src/drivers/md25/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE drivers__md25 - MAIN md25 - COMPILE_FLAGS - SRCS - md25.cpp - md25_main.cpp - DEPENDS - ) - diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp deleted file mode 100644 index 59f6a8a4d5..0000000000 --- a/src/drivers/md25/md25.cpp +++ /dev/null @@ -1,631 +0,0 @@ -/**************************************************************************** - * - * 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 md25.cpp - * - * Driver for MD25 I2C Motor Driver - * - * references: - * http://www.robot-electronics.co.uk/htm/md25tech.htm - * http://www.robot-electronics.co.uk/files/rpi_md25.c - * - */ - -#include "md25.hpp" -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -// registers -enum { - // RW: read/write - // R: read - REG_SPEED1_RW = 0, - REG_SPEED2_RW, - REG_ENC1A_R, - REG_ENC1B_R, - REG_ENC1C_R, - REG_ENC1D_R, - REG_ENC2A_R, - REG_ENC2B_R, - REG_ENC2C_R, - REG_ENC2D_R, - REG_BATTERY_VOLTS_R, - REG_MOTOR1_CURRENT_R, - REG_MOTOR2_CURRENT_R, - REG_SW_VERSION_R, - REG_ACCEL_RATE_RW, - REG_MODE_RW, - REG_COMMAND_RW, -}; - -// File descriptors -static orb_advert_t mavlink_log_pub; - -MD25::MD25(const char *deviceName, int bus, - uint16_t address, uint32_t speed) : - I2C("MD25", deviceName, bus, address, speed), - _controlPoll(), - _actuators(NULL, ORB_ID(actuator_controls_0), 20), - _version(0), - _motor1Speed(0), - _motor2Speed(0), - _revolutions1(0), - _revolutions2(0), - _batteryVoltage(0), - _motor1Current(0), - _motor2Current(0), - _motorAccel(0), - _mode(MODE_UNSIGNED_SPEED), - _command(CMD_RESET_ENCODERS) -{ - // setup control polling - _controlPoll.fd = _actuators.getHandle(); - _controlPoll.events = POLLIN; - - // if initialization fails raise an error, unless - // probing - int ret = I2C::init(); - - if (ret != OK) { - warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address); - } - - // setup default settings, reset encoders - setMotor1Speed(0); - setMotor2Speed(0); - resetEncoders(); - _setMode(MD25::MODE_UNSIGNED_SPEED); - setSpeedRegulation(false); - setMotorAccel(10); - setTimeout(true); -} - -int MD25::readData() -{ - uint8_t sendBuf[1]; - sendBuf[0] = REG_SPEED1_RW; - uint8_t recvBuf[17]; - int ret = transfer(sendBuf, sizeof(sendBuf), - recvBuf, sizeof(recvBuf)); - - if (ret == OK) { - _version = recvBuf[REG_SW_VERSION_R]; - _motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]); - _motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]); - _revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) + - (recvBuf[REG_ENC1B_R] << 16) + - (recvBuf[REG_ENC1C_R] << 8) + - recvBuf[REG_ENC1D_R]) / 360.0; - _revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) + - (recvBuf[REG_ENC2B_R] << 16) + - (recvBuf[REG_ENC2C_R] << 8) + - recvBuf[REG_ENC2D_R]) / 360.0; - _batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0; - _motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0; - _motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0; - _motorAccel = recvBuf[REG_ACCEL_RATE_RW]; - _mode = e_mode(recvBuf[REG_MODE_RW]); - _command = e_cmd(recvBuf[REG_COMMAND_RW]); - } - - return ret; -} - -void MD25::status(char *string, size_t n) -{ - snprintf(string, n, - "version:\t%10d\n" \ - "motor 1 speed:\t%10.2f\n" \ - "motor 2 speed:\t%10.2f\n" \ - "revolutions 1:\t%10.2f\n" \ - "revolutions 2:\t%10.2f\n" \ - "battery volts :\t%10.2f\n" \ - "motor 1 current :\t%10.2f\n" \ - "motor 2 current :\t%10.2f\n" \ - "motor accel :\t%10d\n" \ - "mode :\t%10d\n" \ - "command :\t%10d\n", - getVersion(), - double(getMotor1Speed()), - double(getMotor2Speed()), - double(getRevolutions1()), - double(getRevolutions2()), - double(getBatteryVolts()), - double(getMotor1Current()), - double(getMotor2Current()), - getMotorAccel(), - getMode(), - getCommand()); -} - -uint8_t MD25::getVersion() -{ - return _version; -} - -float MD25::getMotor1Speed() -{ - return _motor1Speed; -} - -float MD25::getMotor2Speed() -{ - return _motor2Speed; -} - -float MD25::getRevolutions1() -{ - return _revolutions1; -} - -float MD25::getRevolutions2() -{ - return _revolutions2; -} - -float MD25::getBatteryVolts() -{ - return _batteryVoltage; -} - -float MD25::getMotor1Current() -{ - return _motor1Current; -} -float MD25::getMotor2Current() -{ - return _motor2Current; -} - -uint8_t MD25::getMotorAccel() -{ - return _motorAccel; -} - -MD25::e_mode MD25::getMode() -{ - return _mode; -} - -MD25::e_cmd MD25::getCommand() -{ - return _command; -} - -int MD25::resetEncoders() -{ - return _writeUint8(REG_COMMAND_RW, - CMD_RESET_ENCODERS); -} - -int MD25::_setMode(e_mode mode) -{ - return _writeUint8(REG_MODE_RW, - mode); -} - -int MD25::setSpeedRegulation(bool enable) -{ - if (enable) { - return _writeUint8(REG_COMMAND_RW, - CMD_ENABLE_SPEED_REGULATION); - - } else { - return _writeUint8(REG_COMMAND_RW, - CMD_DISABLE_SPEED_REGULATION); - } -} - -int MD25::setTimeout(bool enable) -{ - if (enable) { - return _writeUint8(REG_COMMAND_RW, - CMD_ENABLE_TIMEOUT); - - } else { - return _writeUint8(REG_COMMAND_RW, - CMD_DISABLE_TIMEOUT); - } -} - -int MD25::setDeviceAddress(uint8_t address) -{ - uint8_t sendBuf[1]; - sendBuf[0] = CMD_CHANGE_I2C_SEQ_0; - int ret = OK; - ret = transfer(sendBuf, sizeof(sendBuf), - nullptr, 0); - - if (ret != OK) { - warnc(ret, "MD25::setDeviceAddress"); - return ret; - } - - usleep(5000); - sendBuf[0] = CMD_CHANGE_I2C_SEQ_1; - ret = transfer(sendBuf, sizeof(sendBuf), - nullptr, 0); - - if (ret != OK) { - warnc(ret, "MD25::setDeviceAddress"); - return ret; - } - - usleep(5000); - sendBuf[0] = CMD_CHANGE_I2C_SEQ_2; - ret = transfer(sendBuf, sizeof(sendBuf), - nullptr, 0); - - if (ret != OK) { - warnc(ret, "MD25::setDeviceAddress"); - return ret; - } - - return OK; -} - -int MD25::setMotorAccel(uint8_t accel) -{ - return _writeUint8(REG_ACCEL_RATE_RW, - accel); -} - -int MD25::setMotor1Speed(float value) -{ - return _writeUint8(REG_SPEED1_RW, - _normToUint8(value)); -} - -int MD25::setMotor2Speed(float value) -{ - return _writeUint8(REG_SPEED2_RW, - _normToUint8(value)); -} - -void MD25::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; } // poll error - - // if new data, send to motors - if (_actuators.updated()) { - _actuators.update(); - setMotor1Speed(_actuators.control[CH_SPEED_LEFT]); - setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]); - } -} - -int MD25::probe() -{ - uint8_t goodAddress = 0; - bool found = false; - int ret = OK; - - // try initial address first, if good, then done - if (readData() == OK) { return ret; } - - // try all other addresses - uint8_t testAddress = 0; - - //printf("searching for MD25 address\n"); - while (true) { - set_device_address(testAddress); - ret = readData(); - - if (ret == OK && !found) { - //printf("device found at address: 0x%X\n", testAddress); - if (!found) { - found = true; - goodAddress = testAddress; - } - } - - if (testAddress > 254) { - break; - } - - testAddress++; - } - - if (found) { - set_device_address(goodAddress); - return OK; - - } else { - set_device_address(0); - return ret; - } -} - -int MD25::search() -{ - uint8_t goodAddress = 0; - bool found = false; - int ret = OK; - // try all other addresses - uint8_t testAddress = 0; - - //printf("searching for MD25 address\n"); - while (true) { - set_device_address(testAddress); - ret = readData(); - - if (ret == OK && !found) { - printf("device found at address: 0x%X\n", testAddress); - - if (!found) { - found = true; - goodAddress = testAddress; - } - } - - if (testAddress > 254) { - break; - } - - testAddress++; - } - - if (found) { - set_device_address(goodAddress); - return OK; - - } else { - set_device_address(0); - return ret; - } -} - -int MD25::_writeUint8(uint8_t reg, uint8_t value) -{ - uint8_t sendBuf[2]; - sendBuf[0] = reg; - sendBuf[1] = value; - return transfer(sendBuf, sizeof(sendBuf), - nullptr, 0); -} - -int MD25::_writeInt8(uint8_t reg, int8_t value) -{ - uint8_t sendBuf[2]; - sendBuf[0] = reg; - sendBuf[1] = value; - return transfer(sendBuf, sizeof(sendBuf), - nullptr, 0); -} - -float MD25::_uint8ToNorm(uint8_t value) -{ - // TODO, should go from 0 to 255 - // possibly should handle this differently - return (value - 128) / 127.0; -} - -uint8_t MD25::_normToUint8(float value) -{ - if (value > 1) { value = 1; } - - if (value < -1) { value = -1; } - - // TODO, should go from 0 to 255 - // possibly should handle this differently - return 127 * value + 128; -} - -int md25Test(const char *deviceName, uint8_t bus, uint8_t address) -{ - printf("md25 test: starting\n"); - - // setup - MD25 md25("/dev/md25", bus, address); - - // print status - char buf[400]; - md25.status(buf, sizeof(buf)); - printf("%s\n", buf); - - // setup for test - md25.setSpeedRegulation(false); - md25.setTimeout(true); - float dt = 0.1; - float speed = 0.2; - float t = 0; - - // motor 1 test - printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n"); - t = 0; - - while (true) { - t += dt; - md25.setMotor1Speed(speed); - md25.readData(); - usleep(1000000 * dt); - - if (md25.getRevolutions1() > 1) { - printf("finished 1 revolution fwd\n"); - break; - } - - if (t > 2.0f) { break; } - } - - md25.setMotor1Speed(0); - printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); - md25.resetEncoders(); - - t = 0; - - while (true) { - t += dt; - md25.setMotor1Speed(-speed); - md25.readData(); - usleep(1000000 * dt); - - if (md25.getRevolutions1() < -1) { - printf("finished 1 revolution rev\n"); - break; - } - - if (t > 2.0f) { break; } - } - - md25.setMotor1Speed(0); - printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); - md25.resetEncoders(); - - // motor 2 test - printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n"); - t = 0; - - while (true) { - t += dt; - md25.setMotor2Speed(speed); - md25.readData(); - usleep(1000000 * dt); - - if (md25.getRevolutions2() > 1) { - printf("finished 1 revolution fwd\n"); - break; - } - - if (t > 2.0f) { break; } - } - - md25.setMotor2Speed(0); - printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); - md25.resetEncoders(); - - t = 0; - - while (true) { - t += dt; - md25.setMotor2Speed(-speed); - md25.readData(); - usleep(1000000 * dt); - - if (md25.getRevolutions2() < -1) { - printf("finished 1 revolution rev\n"); - break; - } - - if (t > 2.0f) { break; } - } - - md25.setMotor2Speed(0); - printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); - md25.resetEncoders(); - - printf("Test complete\n"); - return 0; -} - -int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency) -{ - printf("md25 sine: starting\n"); - - // setup - MD25 md25("/dev/md25", bus, address); - - // print status - char buf[400]; - md25.status(buf, sizeof(buf)); - printf("%s\n", buf); - - // setup for test - md25.setSpeedRegulation(false); - md25.setTimeout(true); - float dt = 0.01; - float t_final = 60.0; - float prev_revolution = md25.getRevolutions1(); - - // debug publication - uORB::PublicationData debug_msg{ORB_ID(debug_key_value)}; - - // sine wave for motor 1 - md25.resetEncoders(); - - while (true) { - - // input - uint64_t timestamp = hrt_absolute_time(); - float t = timestamp / 1000000.0f; - - float input_value = amplitude * sinf(2 * M_PI * frequency * t); - md25.setMotor1Speed(input_value); - - // output - md25.readData(); - float current_revolution = md25.getRevolutions1(); - - // send input message - //strncpy(debug_msg.key, "md25 in ", 10); - //debug_msg.timestamp_ms = 1000*timestamp; - //debug_msg.value = input_value; - //debug_msg.update(); - - // send output message - strncpy(debug_msg.key, "md25 out ", 10); - debug_msg.timestamp_ms = 1000 * timestamp; - debug_msg.value = current_revolution; - debug_msg.update(); - - if (t > t_final) { break; } - - // update for next step - prev_revolution = current_revolution; - - // sleep - usleep(1000000 * dt); - } - - md25.setMotor1Speed(0); - - printf("md25 sine complete\n"); - return 0; -} - -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp deleted file mode 100644 index f672ae9958..0000000000 --- a/src/drivers/md25/md25.hpp +++ /dev/null @@ -1,309 +0,0 @@ -/**************************************************************************** - * - * 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 md25.cpp - * - * Driver for MD25 I2C Motor Driver - * - * references: - * http://www.robot-electronics.co.uk/htm/md25tech.htm - * http://www.robot-electronics.co.uk/files/rpi_md25.c - * - */ - -#pragma once - -#include -#include -#include -#include -#include - -/** - * This is a driver for the MD25 motor controller utilizing the I2C interface. - */ -class MD25 : public device::I2C -{ -public: - - /** - * modes - * - * NOTE: this driver assumes we are always - * in mode 0! - * - * seprate speed mode: - * motor speed1 = speed1 - * motor speed2 = speed2 - * turn speed mode: - * motor speed1 = speed1 + speed2 - * motor speed2 = speed2 - speed2 - * unsigned: - * full rev (0), stop(128), full fwd (255) - * signed: - * full rev (-127), stop(0), full fwd (128) - * - * modes numbers: - * 0 : unsigned separate (default) - * 1 : signed separate - * 2 : unsigned turn - * 3 : signed turn - */ - enum e_mode { - MODE_UNSIGNED_SPEED = 0, - MODE_SIGNED_SPEED, - MODE_UNSIGNED_SPEED_TURN, - MODE_SIGNED_SPEED_TURN, - }; - - /** commands */ - enum e_cmd { - CMD_RESET_ENCODERS = 32, - CMD_DISABLE_SPEED_REGULATION = 48, - CMD_ENABLE_SPEED_REGULATION = 49, - CMD_DISABLE_TIMEOUT = 50, - CMD_ENABLE_TIMEOUT = 51, - CMD_CHANGE_I2C_SEQ_0 = 160, - CMD_CHANGE_I2C_SEQ_1 = 170, - CMD_CHANGE_I2C_SEQ_2 = 165, - }; - - /** control channels */ - enum e_channels { - CH_SPEED_LEFT = 0, - CH_SPEED_RIGHT - }; - - /** - * constructor - * @param deviceName the name of the device e.g. "/dev/md25" - * @param bus the I2C bus - * @param address the adddress on the I2C bus - * @param speed the speed of the I2C communication - */ - MD25(const char *deviceName, - int bus, - uint16_t address, - uint32_t speed = 100000); - - /** - * deconstructor - */ - virtual ~MD25() = default; - - /** - * @return software version - */ - uint8_t getVersion(); - - /** - * @return speed of motor, normalized (-1, 1) - */ - float getMotor1Speed(); - - /** - * @return speed of motor 2, normalized (-1, 1) - */ - float getMotor2Speed(); - - /** - * @return number of rotations since reset - */ - float getRevolutions1(); - - /** - * @return number of rotations since reset - */ - float getRevolutions2(); - - /** - * @return battery voltage, volts - */ - float getBatteryVolts(); - - /** - * @return motor 1 current, amps - */ - float getMotor1Current(); - - /** - * @return motor 2 current, amps - */ - float getMotor2Current(); - - /** - * @return the motor acceleration - * controls motor speed change (1-10) - * accel rate | time for full fwd. to full rev. - * 1 | 6.375 s - * 2 | 1.6 s - * 3 | 0.675 s - * 5(default) | 1.275 s - * 10 | 0.65 s - */ - uint8_t getMotorAccel(); - - /** - * @return motor output mode - * */ - e_mode getMode(); - - /** - * @return current command register value - */ - e_cmd getCommand(); - - /** - * resets the encoders - * @return non-zero -> error - * */ - int resetEncoders(); - - /** - * enable/disable speed regulation - * @return non-zero -> error - */ - int setSpeedRegulation(bool enable); - - /** - * set the timeout for the motors - * enable/disable timeout (motor stop) - * after 2 sec of no i2c messages - * @return non-zero -> error - */ - int setTimeout(bool enable); - - /** - * sets the device address - * can only be done with one MD25 - * on the bus - * @return non-zero -> error - */ - int setDeviceAddress(uint8_t address); - - /** - * set motor acceleration - * @param accel - * controls motor speed change (1-10) - * accel rate | time for full fwd. to full rev. - * 1 | 6.375 s - * 2 | 1.6 s - * 3 | 0.675 s - * 5(default) | 1.275 s - * 10 | 0.65 s - */ - int setMotorAccel(uint8_t accel); - - /** - * set motor 1 speed - * @param normSpeed normalize speed between -1 and 1 - * @return non-zero -> error - */ - int setMotor1Speed(float normSpeed); - - /** - * set motor 2 speed - * @param normSpeed normalize speed between -1 and 1 - * @return non-zero -> error - */ - int setMotor2Speed(float normSpeed); - - /** - * main update loop that updates MD25 motor - * speeds based on actuator publication - */ - void update(); - - /** - * probe for device - */ - virtual int probe(); - - /** - * search for device - */ - int search(); - - /** - * read data from i2c - */ - int readData(); - - /** - * print status - */ - void status(char *string, size_t n); - -private: - /** poll structure for control packets */ - struct pollfd _controlPoll; - - /** actuator controls subscription */ - uORB::SubscriptionData _actuators; - - // local copy of data from i2c device - uint8_t _version; - float _motor1Speed; - float _motor2Speed; - float _revolutions1; - float _revolutions2; - float _batteryVoltage; - float _motor1Current; - float _motor2Current; - uint8_t _motorAccel; - e_mode _mode; - e_cmd _command; - - // private methods - int _writeUint8(uint8_t reg, uint8_t value); - int _writeInt8(uint8_t reg, int8_t value); - float _uint8ToNorm(uint8_t value); - uint8_t _normToUint8(float value); - - /** - * set motor control mode, - * this driver assumed we are always in mode 0 - * so we don't let the user change the mode - * @return non-zero -> error - */ - int _setMode(e_mode); -}; - -// unit testing -int md25Test(const char *deviceName, uint8_t bus, uint8_t address); - -// sine testing -int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitude, float frequency); - -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp deleted file mode 100644 index a2ffca19f4..0000000000 --- a/src/drivers/md25/md25_main.cpp +++ /dev/null @@ -1,311 +0,0 @@ -/**************************************************************************** - * - * 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 md25.cpp - * - * Driver for MD25 I2C Motor Driver - * - * references: - * http://www.robot-electronics.co.uk/htm/md25tech.htm - * http://www.robot-electronics.co.uk/files/rpi_md25.c - * - */ - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include "md25.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 md25_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int md25_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s", reason); - } - - PX4_INFO("usage: md25 {start|stop|read|status|search|test|change_address}"); - exit(1); -} - -/** - * 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 md25_main(int argc, char *argv[]) -{ - - if (argc < 2) { - usage("missing command"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("md25 already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - deamon_task = px4_task_spawn_cmd("md25", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2048, - md25_thread_main, - (const char **)argv); - exit(0); - } - - if (!strcmp(argv[1], "test")) { - - if (argc < 4) { - printf("usage: md25 test bus address\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t address = strtoul(argv[3], nullptr, 0); - - md25Test(deviceName, bus, address); - - exit(0); - } - - if (!strcmp(argv[1], "sine")) { - - if (argc < 6) { - printf("usage: md25 sine bus address amp freq\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t address = strtoul(argv[3], nullptr, 0); - - float amplitude = atof(argv[4]); - - float frequency = atof(argv[5]); - - md25Sine(deviceName, bus, address, amplitude, frequency); - - exit(0); - } - - if (!strcmp(argv[1], "probe")) { - if (argc < 4) { - printf("usage: md25 probe bus address\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t address = strtoul(argv[3], nullptr, 0); - - MD25 md25(deviceName, bus, address); - - int ret = md25.probe(); - - if (ret == OK) { - printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_device_address()); - - } else { - printf("MD25 not found on bus %d\n", bus); - } - - exit(0); - } - - if (!strcmp(argv[1], "read")) { - if (argc < 4) { - printf("usage: md25 read bus address\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t address = strtoul(argv[3], nullptr, 0); - - MD25 md25(deviceName, bus, address); - - // print status - char buf[400]; - md25.status(buf, sizeof(buf)); - printf("%s\n", buf); - - exit(0); - } - - - if (!strcmp(argv[1], "search")) { - if (argc < 3) { - printf("usage: md25 search bus\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t address = strtoul(argv[3], nullptr, 0); - - MD25 md25(deviceName, bus, address); - - md25.search(); - - exit(0); - } - - if (!strcmp(argv[1], "change_address")) { - if (argc < 5) { - printf("usage: md25 change_address bus old_address new_address\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[2], nullptr, 0); - - uint8_t old_address = strtoul(argv[3], nullptr, 0); - - uint8_t new_address = strtoul(argv[4], nullptr, 0); - - MD25 md25(deviceName, bus, old_address); - - int ret = md25.setDeviceAddress(new_address); - - if (ret == OK) { - printf("MD25 new address set to 0x%X\n", new_address); - - } else { - printf("MD25 failed to set address to 0x%X\n", new_address); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tmd25 app is running\n"); - - } else { - printf("\tmd25 app not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int md25_thread_main(int argc, char *argv[]) -{ - printf("[MD25] starting\n"); - - if (argc < 5) { - // extra md25 in arg list since this is a thread - printf("usage: md25 start bus address\n"); - exit(0); - } - - const char *deviceName = "/dev/md25"; - - uint8_t bus = strtoul(argv[3], nullptr, 0); - - uint8_t address = strtoul(argv[4], nullptr, 0); - - // start - MD25 md25(deviceName, bus, address); - - thread_running = true; - - // loop - while (!thread_should_exit) { - md25.update(); - } - - // exit - printf("[MD25] exiting.\n"); - thread_running = false; - return 0; -} - -// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78