Minor cleanups, added more error verbosity, XXX parameters get now read it at maximum sensors speed, needs to be waiting on a param change notice (but not on the vehicle status topic, as before.

3 files changed, 15 insertions, 6 deletions

diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 3209ee728..8abf427f7 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -522,7 +522,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
 {
-	printf("in update state request\n");
+	printf("[commander] Requested new mode: %d\n", (int)mode);
 	uint8_t ret = 1;
 
 	/* vehicle is disarmed, mode requests arming */
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index c8ef0aebf..f82aa4c52 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -376,7 +376,9 @@ static int parameters_update(const struct sensor_parameter_handles *h, struct se
 	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));
+	if (param_get(h->rc_map_mode_sw, &(p->rc_map_mode_sw)) != OK) {
+		warnx("Loading RC mode sw param failed.");
+	}
 
 	/* gyro offsets */
 	param_get(h->gyro_offset[0], &(p->gyro_offset[0]));
@@ -394,7 +396,9 @@ static int parameters_update(const struct sensor_parameter_handles *h, struct se
 	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));
+	if (param_get(h->battery_voltage_scaling, &(p->battery_voltage_scaling)) != OK) {
+		warnx("Loading voltage scaling param failed.");
+	}
 
 	return OK;
 }
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index f8baa285b..b3fc5b642 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -464,7 +464,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.mag_offset[2], &(_parameters.mag_offset[2]));
 
 	/* scaling of ADC ticks to battery voltage */
-	param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling));
+	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
+		warnx("Failed updating voltage scaling param");
+	}
 
 	return OK;
 }
@@ -604,8 +606,8 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		accel_report.timestamp = hrt_absolute_time();
 
 		accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
-		accel_report.y_raw = (buf[0] == -32768) ? -32767 : buf[0];
-		accel_report.z_raw = (buf[2] == -32768) ? -32767 : buf[2];
+		accel_report.y_raw = buf[0];
+		accel_report.z_raw = buf[2];
 
 		const float range_g = 4.0f;
 		/* scale from 14 bit to m/s2 */
@@ -721,7 +723,10 @@ Sensors::vehicle_status_poll()
 			_hil_enabled = false;
 			_publishing = true;
 		}
+	}
 
+	/* XXX run the param update more often right now */
+	{
 		/* update parameters */
 		parameters_update();