mkclctrl 8/11Bit support, uOrb Topic added, ESC Topic to Sdlog2 added

src/drivers/mkblctrl/mkblctrl.cpp

@@ -74,6 +74,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
 
@@ -87,7 +88,7 @@
 #define MOTOR_STATE_PRESENT_MASK		0x80
 #define MOTOR_STATE_ERROR_MASK		0x7F
 #define MOTOR_SPINUP_COUNTER			2500
-
+#define ESC_UORB_PUBLISH_DELAY			200
 
 class MK : public device::I2C
 {
@@ -119,6 +120,7 @@ public:
 	int		set_pwm_rate(unsigned rate);
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
+	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
 	int		set_px4mode(int px4mode);
 	int		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
@@ -136,11 +138,15 @@ private:
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
+
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
+	orb_advert_t	_t_esc_status;
+
 	unsigned int	_num_outputs;
 	bool		_primary_pwm_device;
 	bool     _motortest;
+	bool 		_overrideSecurityChecks;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -214,6 +220,7 @@ struct MotorData_t {
 	unsigned int Version;                        // the version of the BL (0 = old)
 	unsigned int SetPoint;                       // written by attitude controller
 	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	float SetPoint_PX4; 			     // Values from PX4
 	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
 	unsigned int ReadMode;                       // select data to read
 	unsigned short RawPwmValue;							// length of PWM pulse
@@ -243,8 +250,10 @@ MK::MK(int bus) :
 	_t_armed(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
+	_t_esc_status(0),
 	_num_outputs(0),
 	_motortest(false),
+	_overrideSecurityChecks(false),
 	_motor(-1),
 	_px4mode(MAPPING_MK),
 	_frametype(FRAME_PLUS),
@@ -464,6 +473,13 @@ MK::set_motor_test(bool motortest)
 	return OK;
 }
 
+int
+MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
+{
+	_overrideSecurityChecks = overrideSecurityChecks;
+	return OK;
+}
+
 short
 MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
 {
@@ -506,8 +522,6 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
-
-
 	/* advertise the effective control inputs */
 	actuator_controls_effective_s controls_effective;
 	memset(&controls_effective, 0, sizeof(controls_effective));
@@ -515,6 +529,12 @@ MK::task_main()
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
 				   &controls_effective);
 
+	/* advertise the blctrl status */
+	esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
+
+
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -602,17 +622,22 @@ MK::task_main()
 						}
 					}
 
+					if(!_overrideSecurityChecks) {
 						/* don't go under BLCTRL_MIN_VALUE */
 						if (outputs.output[i] < BLCTRL_MIN_VALUE) {
 							outputs.output[i] = BLCTRL_MIN_VALUE;
 						}
+					}
 
 					/* output to BLCtrl's */
 					if (_motortest == true) {
 						mk_servo_test(i);
 
 					} else {
-						mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+						//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+						// 11 Bit
+						Motor[i].SetPoint_PX4 = outputs.output[i];
+						mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047));	// scale the output to 0 - 2047 and sent to output routine
 					}
 
 				}
@@ -635,8 +660,43 @@ MK::task_main()
 		}
 
 
+
+		/*
+		 * Only update esc topic every half second.
+		 */
+
+		if (hrt_absolute_time() - esc.timestamp > 500000) {
+			esc.counter++;
+			esc.timestamp = hrt_absolute_time();
+			esc.esc_count = (uint8_t) _num_outputs;
+			esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
+
+			for (unsigned int i = 0; i < _num_outputs; i++) {
+				esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
+				esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
+				esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
+				esc.esc[i].esc_voltage = (uint16_t) 0;
+				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
+				esc.esc[i].esc_rpm = (uint16_t) 0;
+				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+				if (Motor[i].Version == 1) {
+					// BLCtrl 2.0 (11Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
+				} else {
+					// BLCtrl < 2.0 (8Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
+				}
+				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
+				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
+				esc.esc[i].esc_errorcount = (uint16_t) 0;
 			}
 
+			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
+		}
+
+	}
+
+	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
 	::close(_t_armed);
@@ -715,17 +775,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
+		
+		if(!_overrideSecurityChecks) {
 			if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
 				_task_should_exit = true;
 			}
 		}
+	}
 
 	return foundMotorCount;
 }
 
-
-
-
 int
 MK::mk_servo_set(unsigned int chan, short val)
 {
@@ -738,17 +798,15 @@ MK::mk_servo_set(unsigned int chan, short val)
 
 	tmpVal = val;
 
-	if (tmpVal > 1024) {
+	if (tmpVal > 2047) {
-		tmpVal = 1024;
+		tmpVal = 2047;
 
 	} else if (tmpVal < 0) {
 		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
+	Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
-	//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
+	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
-
-	Motor[chan].SetPointLowerBits = 0;
 
 	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
@@ -1014,8 +1072,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
-			mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
+			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
-
 		} else {
 			ret = -EINVAL;
 		}
@@ -1112,25 +1169,19 @@ ssize_t
 MK::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	// loeschen uint16_t values[4];
 	uint16_t values[8];
 
-	// loeschen if (count > 4) {
-	// loeschen 	// we only have 4 PWM outputs on the FMU
-	// loeschen	count = 4;
-	// loeschen }
 	if (count > _num_outputs) {
 		// we only have 8 I2C outputs in the driver
 		count = _num_outputs;
 	}
 
-
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
 
 	for (uint8_t i = 0; i < count; i++) {
 		Motor[i].RawPwmValue = (unsigned short)values[i];
-		mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
+		mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
 	}
 
 	return count * 2;
@@ -1282,7 +1333,7 @@ enum FrameType {
 PortMode g_port_mode;
 
 int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
 {
 	uint32_t gpio_bits;
 	int shouldStop = 0;
@@ -1341,6 +1392,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	/* motortest if enabled */
 	g_mk->set_motor_test(motortest);
 
+	/* ovveride security checks if enabled */
+	g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
+
 
 	/* count used motors */
 	do {
@@ -1406,6 +1460,7 @@ mkblctrl_main(int argc, char *argv[])
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
+	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
 
@@ -1461,11 +1516,21 @@ mkblctrl_main(int argc, char *argv[])
 			showHelp = true;
 		}
 
+		/* look for the optional --override-security-checks  parameter */
+		if (strcmp(argv[i], "--override-security-checks") == 0) {
+			overrideSecurityChecks = true;
+			newMode = true;
+		}
+
 	}
 
 	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
-		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
+		fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
+		fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
+		fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
 		exit(1);
 	}
 
@@ -1483,7 +1548,7 @@ mkblctrl_main(int argc, char *argv[])
 	/* parameter set ? */
 	if (newMode) {
 		/* switch parameter */
-		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
+		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
 	}
 
 	/* test, etc. here g*/

src/modules/sdlog2/sdlog2.c

@@ -79,6 +79,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/systemlib.h>
 
@@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 17;
+	const ssize_t fdsc = 18;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
+		struct esc_status_s esc;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
+		int esc_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
 			struct log_GPOS_s log_GPOS;
+			struct log_ESC_s log_ESC;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;	
 
+	/* --- ESCs --- */
+	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
+	fds[fdsc_count].fd = subs.esc_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -1105,6 +1115,27 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(AIRS);
 			}
 
+			/* --- ESCs --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
+				for (uint8_t i=0; i<buf.esc.esc_count; i++)
+				{
+					log_msg.msg_type = LOG_ESC_MSG;
+					log_msg.body.log_ESC.counter = buf.esc.counter;
+					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
+					log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
+					log_msg.body.log_ESC.esc_num = i;
+					log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
+					log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
+					log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
+					log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
+					log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
+					log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
+					log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
+					LOGBUFFER_WRITE_AND_COUNT(ESC);
+				}
+			}
+
 #ifdef SDLOG2_DEBUG
 				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
 #endif

src/modules/sdlog2/sdlog2_messages.h

@@ -209,6 +209,23 @@ struct log_GPOS_s {
 	float vel_e;
 	float vel_d;
 };
+
+/* --- ESC - ESC STATE --- */
+#define LOG_ESC_MSG 64
+struct log_ESC_s {
+	uint16_t counter;
+	uint8_t esc_count;
+	uint8_t esc_connectiontype;
+
+	uint8_t esc_num;
+	uint16_t esc_address;
+	uint16_t esc_voltage;
+	uint16_t esc_current;
+	uint16_t esc_rpm;
+	uint16_t esc_temperature;
+	float    esc_setpoint;
+	uint16_t esc_setpoint_raw;
+};
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -230,6 +247,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+	LOG_FORMAT(ESC, "HBBBHHHHHfH", "Counter,NumESC,Conn,No,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

src/modules/uORB/objects_common.cpp

@@ -160,3 +160,6 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
+
+#include "topics/esc_status.h"
+ORB_DEFINE(esc_status, struct esc_status_s);

src/modules/uORB/topics/esc_status.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Marco Bauer <marco@wtns.de>
+ *
+ * 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 esc_status.h
+ * Definition of the esc_status uORB topic.
+ *
+ * Published the state machine and the system status bitfields
+ * (see SYS_STATUS mavlink message), published only by commander app.
+ *
+ * All apps should write to subsystem_info:
+ *
+ *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> esc_status --> (mavlink app)
+ */
+
+#ifndef ESC_STATUS_H_
+#define ESC_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+/**
+ * The number of ESCs supported.
+ * Current (Q2/2013) we support 8 ESCs,
+ */
+#define CONNECTED_ESC_MAX   8
+
+enum ESC_VENDOR {
+	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
+	ESC_VENDOR_MIKROKOPTER					/**< Mikrokopter */
+};
+
+enum ESC_CONNECTION_TYPE {
+	ESC_CONNECTION_TYPE_PPM = 0,			/**< Traditional PPM ESC */
+	ESC_CONNECTION_TYPE_SERIAL,			/**< Serial Bus connected ESC */
+	ESC_CONNECTION_TYPE_ONESHOOT,			/**< One Shoot PPM */
+	ESC_CONNECTION_TYPE_I2C,				/**< I2C */
+	ESC_CONNECTION_TYPE_CAN					/**< CAN-Bus */
+};
+
+/**
+ *
+ */
+struct esc_status_s
+{
+	/* use of a counter and timestamp recommended (but not necessary) */
+
+	uint16_t counter;   		/**< incremented by the writing thread everytime new data is stored */
+	uint64_t timestamp; 		/**< in microseconds since system start, is set whenever the writing thread stores new data */
+	
+	uint8_t esc_count;			/**< number of connected ESCs */
+	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
+	
+	struct {
+		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
+		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
+		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
+		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
+		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
+		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
+		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
+		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+	} esc[CONNECTED_ESC_MAX];
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+//ORB_DECLARE(esc_status);
+ORB_DECLARE_OPTIONAL(esc_status);
+
+#endif