Bolted new param interface into the sensors app, continuing porting across codebase

1 files changed, 243 insertions, 58 deletions

diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 65ae336d9..37a9e8913 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -46,6 +46,7 @@
 #include <sys/prctl.h>
 #include <nuttx/analog/adc.h>
 #include <unistd.h>
+#include <stdlib.h>
 #include <string.h>
 #include <stdbool.h>
 #include <stdio.h>
@@ -186,21 +187,72 @@ PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
 
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f);
+
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
 
+#define rc_max_chan_count 8
+
+struct sensor_parameters {
+	int min[rc_max_chan_count];
+	int trim[rc_max_chan_count];
+	int max[rc_max_chan_count];
+	int rev[rc_max_chan_count];
+
+	float gyro_offset[3];
+	float mag_offset[3];
+	float acc_offset[3];
+
+	int rc_type;
+
+	int rc_map_roll;
+	int rc_map_pitch;
+	int rc_map_yaw;
+	int rc_map_throttle;
+	int rc_map_mode_sw;
+
+	int battery_voltage_scaling;
+};
+
+struct sensor_parameter_handles {
+	param_t min[rc_max_chan_count];
+	param_t trim[rc_max_chan_count];
+	param_t max[rc_max_chan_count];
+	param_t rev[rc_max_chan_count];
+	param_t rc_type;
+
+	param_t gyro_offset[3];
+	param_t mag_offset[3];
+	param_t acc_offset[3];
+
+	param_t rc_map_roll;
+	param_t rc_map_pitch;
+	param_t rc_map_yaw;
+	param_t rc_map_throttle;
+	param_t rc_map_mode_sw;
+
+	param_t battery_voltage_scaling;
+};
+
+/**
+ * Sensor app start / stop handling function
+ *
+ * @ingroup apps
+ */
+__EXPORT int sensors_main(int argc, char *argv[]);
+
 /**
  * Sensor readout and publishing.
  * 
  * This function reads all onboard sensors and publishes the sensor_combined topic.
  *
  * @see sensor_combined_s
- * @ingroup apps
  */
-__EXPORT int sensors_main(int argc, char *argv[]);
+int sensors_thread_main(int argc, char *argv[]);
 
 /**
  * Print the usage
@@ -208,6 +260,144 @@ __EXPORT int sensors_main(int argc, char *argv[]);
 static void usage(const char *reason);
 
 /**
+ * Initialize all parameter handles and values
+ *
+ */
+static int parameters_init(struct sensor_parameter_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+static int parameters_update(const struct sensor_parameter_handles *h, struct sensor_parameters *p);
+
+
+static int parameters_init(struct sensor_parameter_handles *h)
+{
+	/* min values */
+	h->min[0] = param_find("RC1_MIN");
+	h->min[1] = param_find("RC2_MIN");
+	h->min[2] = param_find("RC3_MIN");
+	h->min[3] = param_find("RC4_MIN");
+	h->min[4] = param_find("RC5_MIN");
+	h->min[5] = param_find("RC6_MIN");
+	h->min[6] = param_find("RC7_MIN");
+	h->min[7] = param_find("RC8_MIN");
+
+	/* trim values */
+	h->trim[0] = param_find("RC1_TRIM");
+	h->trim[1] = param_find("RC2_TRIM");
+	h->trim[2] = param_find("RC3_TRIM");
+	h->trim[3] = param_find("RC4_TRIM");
+	h->trim[4] = param_find("RC5_TRIM");
+	h->trim[5] = param_find("RC6_TRIM");
+	h->trim[6] = param_find("RC7_TRIM");
+	h->trim[7] = param_find("RC8_TRIM");
+
+	/* max values */
+	h->max[0] = param_find("RC1_MAX");
+	h->max[1] = param_find("RC2_MAX");
+	h->max[2] = param_find("RC3_MAX");
+	h->max[3] = param_find("RC4_MAX");
+	h->max[4] = param_find("RC5_MAX");
+	h->max[5] = param_find("RC6_MAX");
+	h->max[6] = param_find("RC7_MAX");
+	h->max[7] = param_find("RC8_MAX");
+
+	/* channel reverse */
+	h->rev[0] = param_find("RC1_REV");
+	h->rev[1] = param_find("RC2_REV");
+	h->rev[2] = param_find("RC3_REV");
+	h->rev[3] = param_find("RC4_REV");
+	h->rev[4] = param_find("RC5_REV");
+	h->rev[5] = param_find("RC6_REV");
+	h->rev[6] = param_find("RC7_REV");
+	h->rev[7] = param_find("RC8_REV");
+
+	h->rc_type = param_find("RC_TYPE");
+
+	h->rc_map_roll 		= param_find("RC_MAP_ROLL");
+	h->rc_map_pitch 	= param_find("RC_MAP_PITCH");
+	h->rc_map_yaw 		= param_find("RC_MAP_YAW");
+	h->rc_map_throttle 	= param_find("RC_MAP_THROTTLE");
+	h->rc_map_mode_sw 	= param_find("RC_MAP_MODE_SW");
+
+	/* gyro offsets */
+	h->gyro_offset[0] = param_find("SENSOR_GYRO_XOFF");
+	h->gyro_offset[1] = param_find("SENSOR_GYRO_YOFF");
+	h->gyro_offset[2] = param_find("SENSOR_GYRO_ZOFF");
+
+	/* accel offsets */
+	h->acc_offset[0] = param_find("SENSOR_ACC_XOFF");
+	h->acc_offset[1] = param_find("SENSOR_ACC_YOFF");
+	h->acc_offset[2] = param_find("SENSOR_ACC_ZOFF");
+
+	/* mag offsets */
+	h->mag_offset[0] = param_find("SENSOR_MAG_XOFF");
+	h->mag_offset[1] = param_find("SENSOR_MAG_YOFF");
+	h->mag_offset[2] = param_find("SENSOR_MAG_ZOFF");
+
+	h->battery_voltage_scaling = param_find("BAT_V_SCALING");
+
+	return OK;
+}
+
+static int parameters_update(const struct sensor_parameter_handles *h, struct sensor_parameters *p)
+{
+	const unsigned int nchans = 8;
+
+	/* min values */
+	for (unsigned int i = 0; i < nchans; i++) {
+		param_get(h->min[i], &(p->min[i]));
+	}
+
+	/* trim values */
+	for (unsigned int i = 0; i < nchans; i++) {
+		param_get(h->trim[i], &(p->trim[i]));
+	}
+
+	/* max values */
+	for (unsigned int i = 0; i < nchans; i++) {
+		param_get(h->max[i], &(p->max[i]));
+	}
+
+	/* channel reverse */
+	for (unsigned int i = 0; i < nchans; i++) {
+		param_get(h->rev[i], &(p->rev[i]));
+	}
+
+	/* remote control type */
+	param_get(h->rc_type, &(p->rc_type));
+
+	/* channel mapping */
+	param_get(h->rc_map_roll, &(p->rc_map_roll));
+	param_get(h->rc_map_pitch, &(p->rc_map_pitch));
+	param_get(h->rc_map_yaw, &(p->rc_map_yaw));
+	param_get(h->rc_map_throttle, &(p->rc_map_throttle));
+	param_get(h->rc_map_mode_sw, &(p->rc_map_mode_sw));
+
+	/* gyro offsets */
+	param_get(h->gyro_offset[0], &(p->gyro_offset[0]));
+	param_get(h->gyro_offset[1], &(p->gyro_offset[1]));
+	param_get(h->gyro_offset[2], &(p->gyro_offset[2]));
+
+	/* accel offsets */
+	param_get(h->acc_offset[0], &(p->acc_offset[0]));
+	param_get(h->acc_offset[1], &(p->acc_offset[1]));
+	param_get(h->acc_offset[2], &(p->acc_offset[2]));
+
+	/* mag offsets */
+	param_get(h->mag_offset[0], &(p->mag_offset[0]));
+	param_get(h->mag_offset[1], &(p->mag_offset[1]));
+	param_get(h->mag_offset[2], &(p->mag_offset[2]));
+
+	/* scaling of ADC ticks to battery voltage */
+	param_get(h->battery_voltage_scaling, &(p->battery_voltage_scaling));
+
+	return OK;
+}
+
+/**
  * Initialize all sensor drivers.
  *
  * @return 0 on success, != 0 on failure
@@ -257,7 +447,6 @@ static int sensors_init(void)
 		printf("[sensors]   Accelerometer open ok\n");
 	}
 
-
 	/* only attempt to use BMA180 if MPU-6000 is not available */
 	int errno_bma180 = 0;
 	if (fd_accelerometer < 0) {
@@ -390,6 +579,12 @@ int sensors_thread_main(int argc, char *argv[])
 		fflush(stdout);
 	}
 
+	/* initialize parameters */
+	struct sensor_parameters rcp;
+	struct sensor_parameter_handles rch;
+	parameters_init(&rch);
+	parameters_update(&rch, &rcp);
+
 	bool gyro_healthy = false;
 	bool acc_healthy = false;
 	bool magn_healthy = false;
@@ -431,10 +626,6 @@ int sensors_thread_main(int argc, char *argv[])
 	int16_t buf_magnetometer[7];
 	float	buf_barometer[3];
 
-	int16_t	mag_offset[3] = {0, 0, 0};
-	int16_t acc_offset[3] = {200, 0, 0};
-	int16_t	gyro_offset[3] = {0, 0, 0};
-
 	bool mag_calibration_enabled = false;
 
 	#pragma pack(push,1)
@@ -454,7 +645,7 @@ int sensors_thread_main(int argc, char *argv[])
 	size_t adc_readsize = 1 * sizeof(struct adc_msg4_s);
 
 	float battery_voltage_conversion;
-	battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION];
+	battery_voltage_conversion = rcp.battery_voltage_scaling;
 
 	if (-1 == (int)battery_voltage_conversion) {
 		/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
@@ -533,7 +724,6 @@ int sensors_thread_main(int argc, char *argv[])
 	memset(&vstatus, 0, sizeof(vstatus));
 	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 
-
 	printf("[sensors] rate: %u Hz\n", (unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC));
 
 	struct hrt_call sensors_hrt_call;
@@ -563,10 +753,9 @@ int sensors_thread_main(int argc, char *argv[])
 			uint64_t current_time = hrt_absolute_time();
 			raw.timestamp = current_time;
 
-			if (paramcounter == 100) {
-				// XXX paramcounter is not a good name, rename / restructure
-				// XXX make counter ticks dependent on update rate of sensor main loop
-
+			/* Update at 5 Hz */
+			if (paramcounter == ((unsigned int)(1000000 / SENSOR_INTERVAL_MICROSEC)/5)) {
+				
 				/* Check HIL state */
 				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
 
@@ -592,46 +781,42 @@ int sensors_thread_main(int argc, char *argv[])
 					publishing = true;
 				}
 
+				/* update parameters */
+				parameters_update(&rch, &rcp);
 
 				/* Update RC scalings and function mappings */
-				rc.chan[0].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2)
-							     * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]);
-				rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM];
+				rc.chan[0].scaling_factor = (1.0f / ((rcp.max[0] - rcp.min[0]) / 2.0f) * rcp.rev[0]);
+				rc.chan[0].mid = rcp.trim[0];
 
-				rc.chan[1].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2)
-							     * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]);
-				rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM];
+				rc.chan[1].scaling_factor = (1.0f / ((rcp.max[1] - rcp.min[1]) / 2.0f) * rcp.rev[1]);
+				rc.chan[1].mid = rcp.trim[1];
 
-				rc.chan[2].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2)
-							     * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]);
-				rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM];
+				rc.chan[2].scaling_factor = (1.0f / ((rcp.max[2] - rcp.min[2]) / 2.0f) * rcp.rev[2]);
+				rc.chan[2].mid = rcp.trim[2];
 
-				rc.chan[3].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2)
-							     * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]);
-				rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM];
+				rc.chan[3].scaling_factor = (1.0f / ((rcp.max[3] - rcp.min[3]) / 2.0f) * rcp.rev[3]);
+				rc.chan[3].mid = rcp.trim[3];
 
-				rc.chan[4].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2)
-							     * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]);
-				rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM];
+				rc.chan[4].scaling_factor = (1.0f / ((rcp.max[4] - rcp.min[4]) / 2.0f) * rcp.rev[4]);
+				rc.chan[4].mid = rcp.trim[4];
 
-				rc.function[0] = global_data_parameter_storage->pm.param_values[PARAM_THROTTLE_CHAN] - 1;
-				rc.function[1] = global_data_parameter_storage->pm.param_values[PARAM_ROLL_CHAN] - 1;
-				rc.function[2] = global_data_parameter_storage->pm.param_values[PARAM_PITCH_CHAN] - 1;
-				rc.function[3] = global_data_parameter_storage->pm.param_values[PARAM_YAW_CHAN] - 1;
-				rc.function[4] = global_data_parameter_storage->pm.param_values[PARAM_OVERRIDE_CHAN] - 1;
+				rc.chan[5].scaling_factor = (1.0f / ((rcp.max[5] - rcp.min[5]) / 2.0f) * rcp.rev[5]);
+				rc.chan[5].mid = rcp.trim[5];
 
-				gyro_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET];
-				gyro_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET];
-				gyro_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET];
+				rc.chan[6].scaling_factor = (1.0f / ((rcp.max[6] - rcp.min[6]) / 2.0f) * rcp.rev[6]);
+				rc.chan[6].mid = rcp.trim[6];
 
-				mag_offset[0] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET];
-				mag_offset[1] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET];
-				mag_offset[2] = global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_ZOFFSET];
+				rc.chan[7].scaling_factor = (1.0f / ((rcp.max[7] - rcp.min[7]) / 2.0f) * rcp.rev[7]);
+				rc.chan[7].mid = rcp.trim[7];
 
+				rc.function[0] = rcp.rc_map_throttle - 1;
+				rc.function[1] = rcp.rc_map_roll - 1;
+				rc.function[2] = rcp.rc_map_pitch - 1;
+				rc.function[3] = rcp.rc_map_yaw - 1;
+				rc.function[4] = rcp.rc_map_mode_sw - 1;
 
 				paramcounter = 0;
 			}
-
 			paramcounter++;
 
 			if (fd_gyro > 0) {
@@ -959,9 +1144,9 @@ int sensors_thread_main(int argc, char *argv[])
 				/* scale measurements */
 				// XXX request scaling from driver instead of hardcoding it
 				/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
-				raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
-				raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
-				raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
+				raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f;
+				raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f;
+				raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f;
 
 				raw.gyro_raw_counter++;
 			}
@@ -974,14 +1159,14 @@ int sensors_thread_main(int argc, char *argv[])
 					/* MPU-6000 values */
 
 					/* scale from 14 bit to m/s2 */
-					raw.accelerometer_m_s2[0] = buf_accel_report.x;
-					raw.accelerometer_m_s2[1] = buf_accel_report.y;
-					raw.accelerometer_m_s2[2] = buf_accel_report.z;
+					raw.accelerometer_m_s2[0] = buf_accel_report.x - rcp.acc_offset[0] / 1000.0f;
+					raw.accelerometer_m_s2[1] = buf_accel_report.y - rcp.acc_offset[1] / 1000.0f;
+					raw.accelerometer_m_s2[2] = buf_accel_report.z - rcp.acc_offset[2] / 1000.0f;
 
 					/* assign negated value, except for -SHORT_MAX, as it would wrap there */
-					raw.accelerometer_raw[0] = buf_accel_report.x*1000; // x of the board is -y of the sensor
-					raw.accelerometer_raw[1] = buf_accel_report.y*1000; // y on the board is x of the sensor
-					raw.accelerometer_raw[2] = buf_accel_report.z*1000; // z of the board is z of the sensor
+					raw.accelerometer_raw[0] = buf_accel_report.x * 1000; // x of the board is -y of the sensor
+					raw.accelerometer_raw[1] = buf_accel_report.y * 1000; // y on the board is x of the sensor
+					raw.accelerometer_raw[2] = buf_accel_report.z * 1000; // z of the board is z of the sensor
 
 					raw.accelerometer_raw_counter++;
 				} else if (fd_bma180 > 0) {
@@ -995,9 +1180,9 @@ int sensors_thread_main(int argc, char *argv[])
 					// XXX read range from sensor
 					float range_g = 4.0f;
 					/* scale from 14 bit to m/s2 */
-					raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
-					raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
-					raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - rcp.acc_offset[0]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - rcp.acc_offset[1]) * range_g) / 8192.0f) / 9.81f;
+					raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - rcp.acc_offset[2]) * range_g) / 8192.0f) / 9.81f;
 
 					raw.accelerometer_raw_counter++;
 				}
@@ -1012,9 +1197,9 @@ int sensors_thread_main(int argc, char *argv[])
 					/* scale measurements */
 					// XXX request scaling from driver instead of hardcoding it
 					/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
-					raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f;
-					raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f;
-					raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f;
+					raw.gyro_rad_s[0] = (raw.gyro_raw[0] - rcp.gyro_offset[0]) * 0.000266316109f;
+					raw.gyro_rad_s[1] = (raw.gyro_raw[1] - rcp.gyro_offset[1]) * 0.000266316109f;
+					raw.gyro_rad_s[2] = (raw.gyro_raw[2] - rcp.gyro_offset[2]) * 0.000266316109f;
 
 					raw.gyro_raw_counter++;
 					/* mark as updated */
@@ -1032,9 +1217,9 @@ int sensors_thread_main(int argc, char *argv[])
 				raw.magnetometer_raw[2] = (buf_magnetometer[2] == -32768) ? -32768 : buf_magnetometer[2]; // z of the board is z of the sensor
 
 				// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
-				raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - mag_offset[0]) / 4096.0f) * 0.88f;
-				raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - mag_offset[1]) / 4096.0f) * 0.88f;
-				raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - mag_offset[2]) / 4096.0f) * 0.88f;
+				raw.magnetometer_ga[0] = ((raw.magnetometer_raw[0] - rcp.mag_offset[0]) / 4096.0f) * 0.88f;
+				raw.magnetometer_ga[1] = ((raw.magnetometer_raw[1] - rcp.mag_offset[1]) / 4096.0f) * 0.88f;
+				raw.magnetometer_ga[2] = ((raw.magnetometer_raw[2] - rcp.mag_offset[2]) / 4096.0f) * 0.88f;
 
 				/* store mode */
 				raw.magnetometer_mode = buf_magnetometer[3];