mkblctrl - rework and bugfix - test ok

1 files changed, 74 insertions, 92 deletions

diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index f3db4dd94..db53b2647 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -95,6 +95,8 @@
 #define MOTOR_LOCATE_DELAY				10000000
 #define ESC_UORB_PUBLISH_DELAY			500000
 
+#define CONTROL_INPUT_DROP_LIMIT_MS		20
+
 struct MotorData_t {
 	unsigned int Version;                        // the version of the BL (0 = old)
 	unsigned int SetPoint;                       // written by attitude controller
@@ -141,57 +143,47 @@ public:
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
-	static const unsigned _max_actuators = MAX_MOTORS;
-	static const bool	showDebug = false;
-
-	int 		_update_rate;
-	int		_task;
-	int		_t_actuators;
-	int		_t_actuator_armed;
-	unsigned int		_motor;
-	int    _px4mode;
-	int    _frametype;
-	char				_device[20];					///< device
-
-	orb_advert_t	_t_outputs;
-	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;
-
-	unsigned long	debugCounter;
-
-	MixerGroup	*_mixers;
-
-	actuator_controls_s _controls;
-
-	MotorData_t Motor[MAX_MOTORS];
-
-	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main();
-
-	static int	control_callback(uintptr_t handle,
-					 uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
-
-	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
-
-
-	int			mk_servo_arm(bool status);
-	int 		mk_servo_set(unsigned int chan, short val);
-	int 		mk_servo_test(unsigned int chan);
-	int 		mk_servo_locate();
-	short		scaling(float val, float inMin, float inMax, float outMin, float outMax);
-	
-	void		play_beep(int count);
-	bool		_indicate_esc; 
-	param_t		_param_indicate_esc;
+	static const unsigned	_max_actuators = MAX_MOTORS;
+	static const bool		showDebug = false;
+
+	int 					_update_rate;
+	int						_task;
+	int						_t_actuators;
+	int						_t_actuator_armed;
+	unsigned int			_motor;
+	int    					_px4mode;
+	int    					_frametype;
+	char					_device[20];
+	orb_advert_t			_t_outputs;
+	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;
+	unsigned long			debugCounter;
+	MixerGroup				*_mixers;
+	bool					_indicate_esc; 
+	param_t					_param_indicate_esc;
+	actuator_controls_s 	_controls;
+	MotorData_t 			Motor[MAX_MOTORS];
+
+	static void				task_main_trampoline(int argc, char *argv[]);
+	void					task_main();
+
+	static int				control_callback(uintptr_t handle,
+								 uint8_t control_group,
+								 uint8_t control_index,
+								 float &input);
+
+	int						pwm_ioctl(file *filp, int cmd, unsigned long arg);
+	int						mk_servo_arm(bool status);
+	int 					mk_servo_set(unsigned int chan, short val);
+	int 					mk_servo_test(unsigned int chan);
+	int 					mk_servo_locate();
+	short					scaling(float val, float inMin, float inMax, float outMin, float outMax);
+	void					play_beep(int count);
 	
 };
 
@@ -202,13 +194,13 @@ const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstrans
 const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 };	// Addresstranslator for Hexa + configuration
 const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 };	// Addresstranslator for Octo + configuration
 
-const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 };	// Addresstranslator for Quad X configuration
-const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 };	// Addresstranslator for Hexa X configuration
-const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };	// Addresstranslator for Octo X configuration
+const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 };		// Addresstranslator for Quad X configuration
+const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 };		// Addresstranslator for Hexa X configuration
+const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 };		// Addresstranslator for Octo X configuration
 
-const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
+const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};			// Native PX4 order - nothing to translate
 
-int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
+int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};					// work copy
 
 namespace
 {
@@ -240,9 +232,7 @@ MK::MK(int bus, const char *_device_path) :
 	strncpy(_device, _device_path, sizeof(_device));
 	/* enforce null termination */
 	_device[sizeof(_device) - 1] = '\0';
-
 	_debug_enabled = true;
-
 }
 
 MK::~MK()
@@ -347,10 +337,10 @@ MK::set_motor_count(unsigned count)
 
 		if (_px4mode == MAPPING_MK) {
 			if (_frametype == FRAME_PLUS) {
-				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: +\n");
+				fprintf(stderr, "[mkblctrl] Mikrokopter ESC addressing. Frame: +\n");
 
 			} else if (_frametype == FRAME_X) {
-				fprintf(stderr, "[mkblctrl] addresstanslator for Mikrokopter addressing used. Frametype: X\n");
+				fprintf(stderr, "[mkblctrl] Mikrokopter ESC addressing. Frame: X\n");
 			}
 
 			if (_num_outputs == 4) {
@@ -379,18 +369,18 @@ MK::set_motor_count(unsigned count)
 			}
 
 		} else {
-			fprintf(stderr, "[mkblctrl] PX4 native addressing used.\n");
+			fprintf(stderr, "[mkblctrl] PX4 ESC addressing.\n");
 			memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4));
 		}
 
 		if (_num_outputs == 4) {
-			fprintf(stderr, "[mkblctrl] Quadrocopter Mode (4)\n");
+			fprintf(stderr, "[mkblctrl] 4 ESCs = Quadrocopter\n");
 
 		} else 	if (_num_outputs == 6) {
-			fprintf(stderr, "[mkblctrl] Hexacopter Mode (6)\n");
+			fprintf(stderr, "[mkblctrl] 6 ESCs = Hexacopter\n");
 
 		} else 	if (_num_outputs == 8) {
-			fprintf(stderr, "[mkblctrl] Octocopter Mode (8)\n");
+			fprintf(stderr, "[mkblctrl] 8 ESCs = Octocopter\n");
 		}
 
 		return OK;
@@ -453,21 +443,27 @@ MK::task_main()
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
 	 * primary PWM output or not.
 	 */
-	 _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0));
-	orb_set_interval(_t_actuators, 20);		/* default to 50Hz */
-
+	_t_actuators = orb_subscribe(ORB_ID(actuator_controls_0));
+	orb_set_interval(_t_actuators, int(1000 / _update_rate));	/* set the topic update rate (200Hz)*/
+	
+	/*
+	 * Subscribe to actuator_armed topic.
+	 */
 	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
 
-	/* advertise the mixed control outputs */
+	/*
+	 * advertise the mixed control outputs.
+	 */
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
 	int dummy;
 	_t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
 				   &outputs, &dummy, ORB_PRIO_HIGH);
 
-	/* advertise the blctrl status */
+	/*
+	 * advertise the blctrl status.
+	 */
 	esc_status_s esc;
 	memset(&esc, 0, sizeof(esc));
 	_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
@@ -479,9 +475,7 @@ MK::task_main()
 	fds[1].fd = _t_actuator_armed;
 	fds[1].events = POLLIN;
 
-
-	orb_set_interval(_t_actuators, int(1000 / _update_rate));
-	up_pwm_servo_set_rate(_update_rate);
+	up_pwm_servo_set_rate(_update_rate);	/* unnecessary ? */
 
 	log("starting");
 
@@ -495,8 +489,8 @@ MK::task_main()
 			_indicate_esc = false;
 		}
 
-		/* sleep waiting for data max 100ms */
-		int ret = ::poll(&fds[0], 2, 100);
+		/* waiting for data */
+		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -525,7 +519,6 @@ MK::task_main()
 
 					/* iterate actuators */
 					for (unsigned int i = 0; i < _num_outputs; i++) {
-
 						/* last resort: catch NaN, INF and out-of-band errors */
 						if (i < outputs.noutputs &&
 						    isfinite(outputs.output[i]) &&
@@ -552,11 +545,9 @@ 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 */
@@ -564,13 +555,9 @@ MK::task_main()
 							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
 						}
-
 					}
-
 				}
 			}
-
-
 		}
 
 		/* how about an arming update? */
@@ -584,8 +571,6 @@ MK::task_main()
 			mk_servo_arm(aa.armed && !aa.lockdown);
 		}
 
-
-
 		/*
 		 * Only update esc topic every half second.
 		 */
@@ -608,7 +593,6 @@ MK::task_main()
 				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;
@@ -618,7 +602,7 @@ MK::task_main()
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
 
-				// if motortest is requested - do it...
+				// if motortest is requested - do it... (deprecated in future)
 				if (_motortest == true) {
 					mk_servo_test(i);
 				}
@@ -627,7 +611,6 @@ MK::task_main()
 				if (_indicate_esc == true) {
 					mk_servo_locate();
 				}
-
 			}
 
 			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
@@ -1237,7 +1220,7 @@ mkblctrl_main(int argc, char *argv[])
 			if (argc > i + 1) {
 				if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) {
 					px4mode = MAPPING_MK;
-					newMode = true;
+					//newMode = true;
 
 					if (strcmp(argv[i + 1], "+") == 0) {
 						frametype = FRAME_PLUS;
@@ -1259,26 +1242,25 @@ mkblctrl_main(int argc, char *argv[])
 		/* look for the optional test parameter */
 		if (strcmp(argv[i], "-t") == 0) {
 			motortest = true;
-			newMode = true;
+			//newMode = true;
 		}
 
 		/* look for the optional -h --help parameter */
 		if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) {
 			showHelp = true;
-			newMode = false;
 		}
 
 		/* look for the optional --override-security-checks  parameter */
 		if (strcmp(argv[i], "--override-security-checks") == 0) {
 			overrideSecurityChecks = true;
-			newMode = true;
+			//newMode = true;
 		}
 
 		/* look for the optional device parameter */
 		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
 			if (argc > i + 1) {
 				devicepath = argv[i + 1];
-				newMode = true;
+				//newMode = true;
 
 			} else {
 				errx(1, "missing the devicename (-d)");