Mixer library: Fix code style

src/modules/px4iofirmware/mixer.cpp

@@ -35,6 +35,8 @@
  * @file mixer.cpp
  *
  * Control channel input/output mixer and failsafe.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include <nuttx/config.h>
@@ -64,6 +66,7 @@ extern "C" {
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
 static bool should_arm = false;
+static bool should_arm_nothrottle = false;
 static bool should_always_enable_pwm = false;
 static volatile bool in_mixer = false;
 
@@ -172,6 +175,11 @@ mixer_tick(void)
 						     )
 	);
 
+	should_arm_nothrottle = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
+
 	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
 						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
@@ -237,24 +245,25 @@ mixer_tick(void)
 		/* the pwm limit call takes care of out of band errors */
 		pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
-		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+		/* clamp unused outputs to zero */
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = 0;
+			outputs[i] = 0;
+		}
 
+		/* store normalized outputs */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 		}
 	}
 
 	/* set arming */
-	bool needs_to_arm = (should_arm || should_always_enable_pwm);
+	bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
 
 	/* check any conditions that prevent arming */
 	if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
 		needs_to_arm = false;
 	}
-	if (!should_arm && !should_always_enable_pwm) {
-		needs_to_arm = false;
-	}
 
 	if (needs_to_arm && !mixer_servos_armed) {
 		/* need to arm, but not armed */
@@ -308,8 +317,9 @@ mixer_callback(uintptr_t handle,
 	       uint8_t control_index,
 	       float &control)
 {
-	if (control_group >= PX4IO_CONTROL_GROUPS)
+	if (control_group >= PX4IO_CONTROL_GROUPS) {
 		return -1;
+	}
 
 	switch (source) {
 	case MIX_FMU:
@@ -359,8 +369,8 @@ mixer_callback(uintptr_t handle,
 		}
 	}
 
-	/* motor spinup phase - lock throttle to zero */
+	/* motor spinup phase or only safety off, but not armed - lock throttle to zero */
-	if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
+	if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
 		if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
 			control_index == actuator_controls_s::INDEX_THROTTLE) {
 			/* limit the throttle output to zero during motor spinup,
@@ -458,8 +468,9 @@ mixer_handle_text(const void *buffer, size_t length)
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
-			if (resid > 0)
+			if (resid > 0) {
 				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+			}
 
 			mixer_text_length = resid;
 
@@ -482,8 +493,9 @@ mixer_set_failsafe()
 	 */
 
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
-		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
 		return;
+	}
 
 	/* set failsafe defaults to the values for all inputs = 0 */
 	float	outputs[PX4IO_SERVO_COUNT];
@@ -501,7 +513,8 @@ mixer_set_failsafe()
 	}
 
 	/* disable the rest of the outputs */
-	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
 		r_page_servo_failsafe[i] = 0;
+	}
 
 }

src/modules/systemlib/mixer/mixer.cpp

@@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input)
 int
 Mixer::scale_check(struct mixer_scaler_s &scaler)
 {
-	if (scaler.offset > 1.001f)
+	if (scaler.offset > 1.001f) {
 		return 1;
+	}
 
-	if (scaler.offset < -1.001f)
+	if (scaler.offset < -1.001f) {
 		return 2;
+	}
 
-	if (scaler.min_output > scaler.max_output)
+	if (scaler.min_output > scaler.max_output) {
 		return 3;
+	}
 
-	if (scaler.min_output < -1.001f)
+	if (scaler.min_output < -1.001f) {
 		return 4;
+	}
 
-	if (scaler.max_output > 1.001f)
+	if (scaler.max_output > 1.001f) {
 		return 5;
+	}
 
 	return 0;
 }
@@ -120,11 +125,14 @@ const char *
 Mixer::findtag(const char *buf, unsigned &buflen, char tag)
 {
 	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
+		if ((buf[0] == tag) && (buf[1] == ':')) {
 			return buf;
+		}
+
 		buf++;
 		buflen--;
 	}
+
 	return nullptr;
 }
 
@@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen)
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
+		if (buf[i] == '\0') {
 			continue;
+		}
 
 		/* require a space or newline at the end of the buffer, fail on printable chars */
 		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
 			/* found a line ending or space, so no split symbols / numbers. good. */
 			break;
+
 		} else {
 			return nm;
 		}

src/modules/systemlib/mixer/mixer_load.c

@@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 	/* open the mixer definition file */
 	fp = fopen(fname, "r");
+
 	if (fp == NULL) {
 		warnx("file not found");
 		return -1;
@@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 
 	/* read valid lines from the file into a buffer */
 	buf[0] = '\0';
+
 	for (;;) {
 
 		/* get a line, bail on error/EOF */
 		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
+
+		if (fgets(line, sizeof(line), fp) == NULL) {
 			break;
+		}
 
 		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
 			continue;
+		}
 
 		/* compact whitespace in the buffer */
 		char *t, *f;
+
 		for (f = line; *f != '\0'; f++) {
 			/* scan for space characters */
 			if (*f == ' ') {
 				/* look for additional spaces */
 				t = f + 1;
-				while (*t == ' ')
+
+				while (*t == ' ') {
 					t++;
+				}
+
 				if (*t == '\0') {
 					/* strip trailing whitespace */
 					*f = '\0';
+
 				} else if (t > (f + 1)) {
 					memmove(f + 1, t, strlen(t) + 1);
 				}

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -109,13 +109,15 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 
 	/* enforce that the mixer ends with space or a new line */
 	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
+		if (buf[i] == '\0') {
 			continue;
+		}
 
 		/* require a space or newline at the end of the buffer, fail on printable chars */
 		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
 			/* found a line ending or space, so no split symbols / numbers. good. */
 			break;
+
 		} else {
 			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]);
 			return nullptr;
@@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return nullptr;
@@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	if (status_reg != NULL) {
 		(*status_reg) = 0;
 	}
+
 	// thrust boost parameters
 	float thrust_increase_factor = 1.5f;
 	float thrust_decrease_factor = 0.6f;
@@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		if (out < min_out) {
 			min_out = out;
 		}
+
 		if (out > max_out) {
 			max_out = out;
 		}
@@ -250,35 +255,39 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 
 	if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
 		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
+
 		if (max_thrust_diff >= -min_out) {
 			boost = -min_out;
-		}
+
-		else {
+		} else {
 			boost = max_thrust_diff;
 			roll_pitch_scale = (thrust + boost) / (thrust - min_out);
 		}
-	}
+
-	else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
+	} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
 		float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
+
 		if (max_thrust_diff >= max_out - 1.0f) {
 			boost = -(max_out - 1.0f);
+
 		} else {
 			boost = -max_thrust_diff;
 			roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
 		}
-	}
+
-	else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
+	} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
 		float max_thrust_diff = thrust * thrust_increase_factor - thrust;
 		boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
 		roll_pitch_scale = (thrust + boost) / (thrust - min_out);
-	}
+
-	else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
+	} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
 		float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
 		boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
 		roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
-	}
+
-	else if (min_out < 0.0f && max_out > 1.0f) {
+	} else if (min_out < 0.0f && max_out > 1.0f) {
-		boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
+		boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
+				  thrust_increase_factor * thrust - thrust);
 		roll_pitch_scale = (thrust + boost) / (thrust - min_out);
 	}
 
@@ -288,6 +297,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
 		}
 	}
+
 	if (max_out > 0.0f) {
 		if (status_reg != NULL) {
 			(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
@@ -307,23 +317,25 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 		if (out < 0.0f) {
 			yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
 				roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
+
 			if (status_reg != NULL) {
 				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
 			}
-		}
+
-		else if(out > 1.0f) {
+		} else if (out > 1.0f) {
 			// allow to reduce thrust to get some yaw response
 			float thrust_reduction = fminf(0.15f, out - 1.0f);
 			thrust -= thrust_reduction;
 			yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
 				       roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
+
 			if (status_reg != NULL) {
 				(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
 			}
 		}
 	}
 
-	/* last mix, add yaw and scale outputs to range idle_speed...1 */
+	/* add yaw and scale outputs to range idle_speed...1 */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		outputs[i] = (roll * _rotors[i].roll_scale +
 			      pitch * _rotors[i].pitch_scale) * roll_pitch_scale +

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -67,9 +67,10 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb,
 
 SimpleMixer::~SimpleMixer()
 {
-	if (_info != nullptr)
+	if (_info != nullptr) {
 		free(_info);
 	}
+}
 
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
@@ -79,6 +80,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 	int n = -1;
 
 	buf = findtag(buf, buflen, 'O');
+
 	if ((buf == nullptr) || (buflen < 12)) {
 		debug("output parser failed finding tag, ret: '%s'", buf);
 		return -1;
@@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return -1;
@@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 }
 
 int
-SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
+SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
+				  uint8_t &control_index)
 {
 	unsigned u[2];
 	int s[5];
 
 	buf = findtag(buf, buflen, 'S');
+
 	if ((buf == nullptr) || (buflen < 16)) {
 		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
@@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		return -1;
@@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	}
 
 	buf = skipline(buf, buflen);
+
 	if (buf == nullptr) {
 		debug("no line ending, line is incomplete");
 		goto out;
@@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 out:
 
-	if (mixinfo != nullptr)
+	if (mixinfo != nullptr) {
 		free(mixinfo);
+	}
 
 	return sm;
 }
 
 SimpleMixer *
-SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
+SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
+		       uint16_t mid, uint16_t max)
 {
 	SimpleMixer *sm = nullptr;
 	mixer_simple_s *mixinfo = nullptr;
@@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u
 
 out:
 
-	if (mixinfo != nullptr)
+	if (mixinfo != nullptr) {
 		free(mixinfo);
+	}
 
 	return sm;
 }
@@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 {
 	float		sum = 0.0f;
 
-	if (_info == nullptr)
+	if (_info == nullptr) {
 		return 0;
+	}
 
-	if (space < 1)
+	if (space < 1) {
 		return 0;
+	}
 
 	for (unsigned i = 0; i < _info->control_count; i++) {
 		float input;
@@ -293,9 +305,10 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 void
 SimpleMixer::groups_required(uint32_t &groups)
 {
-	for (unsigned i = 0; i < _info->control_count; i++)
+	for (unsigned i = 0; i < _info->control_count; i++) {
 		groups |= 1 << _info->controls[i].control_group;
 	}
+}
 
 int
 SimpleMixer::check()
@@ -305,14 +318,16 @@ SimpleMixer::check()
 
 	/* sanity that presumes that a mixer includes a control no more than once */
 	/* max of 32 groups due to groups_required API */
-	if (_info->control_count > 32)
+	if (_info->control_count > 32) {
 		return -2;
+	}
 
 	/* validate the output scaler */
 	ret = scale_check(_info->output_scaler);
 
-	if (ret != 0)
+	if (ret != 0) {
 		return ret;
+	}
 
 	/* validate input scalers */
 	for (unsigned i = 0; i < _info->control_count; i++) {
@@ -328,9 +343,10 @@ SimpleMixer::check()
 		/* validate the scaler */
 		ret = scale_check(_info->controls[i].scaler);
 
-		if (ret != 0)
+		if (ret != 0) {
 			return (10 * i + ret);
 		}
+	}
 
 	return 0;
 }