Merge pull request #485 from jgoppert/roboclaw

Roboclaw

ROMFS/px4fmu_common/init.d/40_io_segway

@@ -52,5 +52,5 @@ attitude_estimator_ekf start
 #
 # Load mixer and start controllers (depends on px4io)
 #
-md25 start 3 0x58
+roboclaw start /dev/ttyS2 128 1200
 segway start

ROMFS/px4fmu_common/init.d/rcS

@@ -272,13 +272,19 @@ then
 		sh /etc/init.d/30_io_camflyer
 		set MODE custom
 	fi
-	
+
 	if param compare SYS_AUTOSTART 31
 	then
 		sh /etc/init.d/31_io_phantom
 		set MODE custom
 	fi
 
+	if param compare SYS_AUTOSTART 40
+	then
+		sh /etc/init.d/40_io_segway
+		set MODE custom
+	fi
+	
 	if param compare SYS_AUTOSTART 100
 	then
 		sh /etc/init.d/100_mpx_easystar

makefiles/config_px4fmu-v1_default.mk

@@ -33,7 +33,7 @@ MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
-MODULES		+= drivers/md25
+MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
@@ -79,7 +79,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX needs state machine update
+MODULES		+= modules/segway
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control

makefiles/config_px4fmu-v2_default.mk

@@ -31,6 +31,7 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
@@ -77,7 +78,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX needs state machine update
+MODULES		+= modules/segway
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control

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

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

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

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

src/modules/segway/BlockSegwayController.cpp

@@ -26,7 +26,7 @@ void BlockSegwayController::update() {
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.main_state == MAIN_STATE_AUTO) {
 		// update guidance
 	}
 
@@ -34,17 +34,18 @@ void BlockSegwayController::update() {
 	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+	if (_status.main_state == MAIN_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	    _status.main_state == MAIN_STATE_SEATBELT ||
+	    _status.main_state == MAIN_STATE_EASY) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
 			_actuators.control[CH_LEFT] = _manual.throttle;
 			_actuators.control[CH_RIGHT] = _manual.pitch;
 
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
 			_actuators.control[0] = spdCmd;
 			_actuators.control[1] = spdCmd;
 		}