IO Firmware: Code style fix, fix RSSI ADC lowpass

src/modules/px4iofirmware/controls.c

@@ -63,6 +63,7 @@ static perf_counter_t c_gather_ppm;
 static int _dsm_fd;
 
 static uint16_t rc_value_override = 0;
+static unsigned _rssi_adc_counts = 0;
 
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
@@ -71,17 +72,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+
 	if (*dsm_updated) {
 		r_raw_rc_count = temp_count & 0x7fff;
-		if (temp_count & 0x8000)
+
+		if (temp_count & 0x8000) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-		else
+
+		} else {
 			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+		}
 
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 
 	}
+
 	perf_end(c_gather_dsm);
 
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
@@ -94,7 +100,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		/* set updated flag if one complete packet was parsed */
 		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
-					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+						    &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*st24_updated) {
@@ -121,7 +127,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		/* set updated flag if one complete packet was parsed */
 		sumd_rssi = RC_INPUT_RSSI_MAX;
 		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
-					&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+						    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*sumd_updated) {
@@ -170,7 +176,8 @@ controls_init(void)
 }
 
 void
-controls_tick() {
+controls_tick()
+{
 
 	/*
 	 * Gather R/C control inputs from supported sources.
@@ -184,19 +191,24 @@ controls_tick() {
 	uint16_t rssi = 0;
 
 #ifdef ADC_RSSI
+
 	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
 		unsigned counts = adc_measure(ADC_RSSI);
+
 		if (counts != 0xffff) {
-			/* use 1:1 scaling on 3.3V ADC input */
-			unsigned mV = counts * 3300 / 4096;
+			/* low pass*/
+			_rssi_adc_counts = (_rssi_adc_counts * 0.99f) + (counts * 0.01f);
+			/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
+			unsigned mV = _rssi_adc_counts * 3300 / 4095;
+			/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
+			rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
 
-			/* scale to 0..253 and lowpass */
-			rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
 			if (rssi > RC_INPUT_RSSI_MAX) {
 				rssi = RC_INPUT_RSSI_MAX;
 			}
 		}
 	}
+
 #endif
 
 	/* zero RSSI if signal is lost */
@@ -207,21 +219,26 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated, sumd_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
+
 	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 	}
+
 	if (st24_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
 	}
+
 	if (sumd_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
 	}
+
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
 
 	bool sbus_failsafe, sbus_frame_drop;
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
+				       PX4IO_RC_INPUT_CHANNELS);
 
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
@@ -231,17 +248,19 @@ controls_tick() {
 		if (sbus_frame_drop) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
 			sbus_rssi = RC_INPUT_RSSI_MAX / 2;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		}
 
 		if (sbus_failsafe) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 		}
 
-		/* set RSSI to an emulated value if ADC RSSI is off */ 
+		/* set RSSI to an emulated value if ADC RSSI is off */
 		if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
 			rssi = sbus_rssi;
 		}
@@ -257,17 +276,20 @@ controls_tick() {
 	 */
 	perf_begin(c_gather_ppm);
 	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
+
 	if (ppm_updated) {
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
+
 	perf_end(c_gather_ppm);
 
 	/* limit number of channels to allowable data size */
-	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+	}
 
 	/* store RSSI */
 	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
@@ -308,10 +330,13 @@ controls_tick() {
 				/*
 				 * 1) Constrain to min/max values, as later processing depends on bounds.
 				 */
-				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
 					raw = conf[PX4IO_P_RC_CONFIG_MIN];
-				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+				}
+
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
 					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+				}
 
 				/*
 				 * 2) Scale around the mid point differently for lower and upper range.
@@ -330,10 +355,12 @@ controls_tick() {
 				 * DO NOT REMOVE OR ALTER STEP 1!
 				 */
 				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
+								     conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
 
 				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
+								     conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
 
 				} else {
 					/* in the configured dead zone, output zero */
@@ -347,6 +374,7 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+
 				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
 					/* invert channel if pitch - pulling the lever down means pitching up by convention */
@@ -360,6 +388,7 @@ controls_tick() {
 						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
 						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
 							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+
 						} else {
 							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 						}
@@ -389,6 +418,7 @@ controls_tick() {
 		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
 		if (assigned_channels > 4) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
@@ -408,9 +438,9 @@ controls_tick() {
 
 		/* clear the input-kind flags here */
 		r_status_flags &= ~(
-			PX4IO_P_STATUS_FLAGS_RC_PPM |
-			PX4IO_P_STATUS_FLAGS_RC_DSM |
-			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+					  PX4IO_P_STATUS_FLAGS_RC_PPM |
+					  PX4IO_P_STATUS_FLAGS_RC_DSM |
+					  PX4IO_P_STATUS_FLAGS_RC_SBUS);
 
 	}
 
@@ -427,8 +457,8 @@ controls_tick() {
 	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
-			PX4IO_P_STATUS_FLAGS_OVERRIDE |
-			PX4IO_P_STATUS_FLAGS_RC_OK);
+					  PX4IO_P_STATUS_FLAGS_OVERRIDE |
+					  PX4IO_P_STATUS_FLAGS_RC_OK);
 
 		/* flag raw RC as lost */
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
@@ -451,9 +481,9 @@ controls_tick() {
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
-	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
+	    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -464,8 +494,9 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
 			override = true;
+		}
 
 		/*
 		  if the FMU is dead then enable override if we have a
@@ -473,20 +504,23 @@ controls_tick() {
 		 */
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
-		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
-			override = true;                
+		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+			override = true;
+		}
 
 		if (override) {
 
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
 				mixer_tick();
+			}
 
 		} else {
 			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
+
 	} else {
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
@@ -512,8 +546,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS) {
 			*num_values = PX4IO_RC_INPUT_CHANNELS;
+		}
 
 		for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
 			values[i] = ppm_buffer[i];