Various minor fixes and improvements across system

7 files changed, 89 insertions, 56 deletions

diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 380ae9c15..2644a25cb 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -43,6 +43,8 @@
 #include <unistd.h>
 #include <drivers/drv_gpio.h>
 #include <arch/board/up_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_outputs.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -270,29 +272,33 @@ int ar_init_motors(int ardrone_uart, int gpios)
 		usleep(UART_TRANSFER_TIME_BYTE_US*11);
 
 		ar_deselect_motor(gpios, i);
-		/* sleep 400 ms */
-		usleep(400000);
+		/* sleep 200 ms */
+		usleep(200000);
 	}
 
 	/* start the multicast part */
 	errcounter += ar_select_motor(gpios, 0);
 
 	/*
+	 * first round
 	 * write six times A0 - enable broadcast
 	 * receive nothing
 	 */
-	// for (int i = 0; i < sizeof(multicastbuf); i++) {
-	write(ardrone_uart, multicastbuf, 6);
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
 	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
 
-	usleep(200000);
-
-	write(ardrone_uart, multicastbuf, 6);
+	/*
+	 * second round
+	 * write six times A0 - enable broadcast
+	 * receive nothing
+	 */
+	write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
 	fsync(ardrone_uart);
+	usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
 
-	/* set motors to zero speed */
+	/* set motors to zero speed (fsync is part of the write command */
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
-	fsync(ardrone_uart);
 
 	if (errcounter != 0) {
 		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@@ -324,12 +330,27 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
 int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
 	const unsigned int min_motor_interval = 4900;
 	static uint64_t last_motor_time = 0;
+
+	static struct actuator_outputs_s outputs;
+	outputs.output[0] = motor1;
+	outputs.output[1] = motor2;
+	outputs.output[2] = motor3;
+	outputs.output[3] = motor4;
+	static orb_advert_t pub = 0;
+	if (pub == 0) {
+		pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	}
+
 	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
 		uint8_t buf[5] = {0};
 		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
 		int ret;
 		ret = write(ardrone_fd, buf, sizeof(buf));
 		fsync(ardrone_fd);
+
+		/* publish just written values */
+		orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
+
 		if (ret == sizeof(buf)) {
 			return OK;
 		} else {
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 4135abf60..491a658a1 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -973,7 +973,7 @@ int commander_thread_main(int argc, char *argv[])
 	uint8_t flight_env;
 
 	/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
-	float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
+	float battery_voltage = 12.0f;
 	bool battery_voltage_valid = true;
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
@@ -1010,6 +1010,8 @@ int commander_thread_main(int argc, char *argv[])
 	/* now initialized */
 	commander_initialized = true;
 
+	uint64_t start_time = hrt_absolute_time();
+
 	while (!thread_should_exit) {
 
 		/* Get current values */
@@ -1019,7 +1021,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		battery_voltage = sensors.battery_voltage_v;
 		battery_voltage_valid = sensors.battery_voltage_valid;
-		bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+
+		/*
+		 * Only update battery voltage estimate if voltage is
+		 * valid and system has been running for half a second
+		 */
+		if (battery_voltage_valid && (hrt_absolute_time() - start_time > 500000)) {
+			bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+		}
 
 		/* Slow but important 8 Hz checks */
 		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
@@ -1084,6 +1093,38 @@ int commander_thread_main(int argc, char *argv[])
 			ioctl(buzzer, TONE_SET_ALARM, 0);
 		}
 
+		/* Check battery voltage */
+		/* write to sys_status */
+		current_status.voltage_battery = battery_voltage;
+
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
+		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+
+			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				low_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
+			}
+
+			low_voltage_counter++;
+		}
+
+		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
+		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
+				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
+			}
+
+			critical_voltage_counter++;
+
+		} else {
+			low_voltage_counter = 0;
+			critical_voltage_counter = 0;
+		}
+
+		/* End battery voltage check */
+
 		/* Check if last transition deserved an audio event */
 #warning This code depends on state that is no longer? maintained
 #if 0
@@ -1142,37 +1183,6 @@ int commander_thread_main(int argc, char *argv[])
 		//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
 		/* end: check gps */
 
-		/* Check battery voltage */
-		/* write to sys_status */
-		current_status.voltage_battery = battery_voltage;
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
 
 		/* Start RC state check */
 		bool prev_lost = current_status.rc_signal_lost;
diff --git a/apps/commander/commander.h b/apps/commander/commander.h
index d537a0602..e9e3b24ca 100644
--- a/apps/commander/commander.h
+++ b/apps/commander/commander.h
@@ -39,8 +39,6 @@
 #ifndef COMMANDER_H_
 #define COMMANDER_H_
 
-#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f
-#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f
 #define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
 #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
 #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp
index ad4976bc2..7b8a84f7e 100644
--- a/apps/drivers/mpu6000/mpu6000.cpp
+++ b/apps/drivers/mpu6000/mpu6000.cpp
@@ -369,8 +369,10 @@ MPU6000::init()
 	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
 	usleep(1000);
 
-	// FS & DLPF   FS=2000 deg/s, DLPF = 98Hz (low pass filter)
-	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
+	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
+	// was 90 Hz, but this ruins quality and does not improve the
+	// system response
+	write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ);
 	usleep(1000);
 	// Gyro scale 2000 deg/s ()
 	write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 0d9be58e9..1d71b4019 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -542,6 +542,7 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
 			if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
 			if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
 			if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
+			if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_MANUAL_CONTROL:
 			/* manual_control_setpoint triggers this message */
@@ -712,7 +713,6 @@ static void *uorb_receiveloop(void *arg)
 	/* --- ACTUATOR CONTROL VALUE --- */
 	/* subscribe to ORB for actuator control */
 	subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(subs->actuators_sub, 2000);	/* 0.5 Hz updates */
 	fds[fdsc_count].fd = subs->actuators_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -1518,23 +1518,24 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 921600) {
 		/* 500 Hz / 2 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
 		/* 200 Hz / 5 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
 		/* 5 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
 	} else if (baudrate >= 460800) {
-		/* 250 Hz / 4 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
+		/* 200 Hz / 5 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
 		/* 100 Hz / 10 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 10);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 10);
+		/* 66 Hz / 15  ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 15);
+		/* 20 Hz / 20 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
 		/* 2 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else if (baudrate >= 115200) {
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 4f5082bed..54e25bba0 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -144,6 +144,7 @@ mc_thread_main(int argc, char *argv[])
 		att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
 		att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
 		att_sp.yaw_body = -manual.yaw * M_PI_F;
+		att_sp.timestamp = hrt_absolute_time();
 		if (motor_test_mode) {
 			att_sp.roll_body = 0.0f;
 			att_sp.pitch_body = 0.0f;
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 224474c08..c5b9e483e 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -855,7 +855,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 			/* Voltage in volts */
 			raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
 
-			if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
+			if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
 				raw.battery_voltage_valid = false;
 				raw.battery_voltage_v = 0.f;