Merge branch 'master' into beta

ROMFS/px4fmu_common/init.d/13002_firefly6

@@ -8,7 +8,7 @@
 sh /etc/init.d/rc.vtol_defaults
 
 set MIXER firefly6
-set PWM_OUT 123456
+set PWM_OUT 12345678
 
 set MIXER_AUX firefly6
 set PWM_AUX_RATE 50

ROMFS/px4fmu_common/init.d/rcS

@@ -461,10 +461,16 @@ then
 	#
 	if ver hwcmp PX4FMU_V2
 	then
+		# XXX We need a better way for runtime eval of shell variables,
+		# but this works for now
 		if param compare SYS_COMPANION 921600
 		then
 			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
 		fi
+		if param compare SYS_COMPANION 57600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
+		fi
 	fi
 
 	# UAVCAN

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 						math::Vector<3> v(1.0f, 0.0f, 0.4f);
 
-						math::Vector<3> vn = Rvis * v;
+						math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
-
+											    // Rrw * Vw = vn. This way we have consistency
 						z_k[6] = vn(0);
 						z_k[7] = vn(1);
 						z_k[8] = vn(2);

src/modules/commander/PreflightCheck.cpp

@@ -93,7 +93,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	if (devid != calibration_devid) {
 		mavlink_and_console_log_critical(mavlink_fd,
-						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
+						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		success = false;
 		goto out;
 	}
@@ -137,7 +137,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 
 	if (devid != calibration_devid) {
 		mavlink_and_console_log_critical(mavlink_fd,
-						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
+						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		success = false;
 		goto out;
 	}
@@ -161,7 +161,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 			float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 			if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
+				mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
 				/* this is frickin' fatal */
 				success = false;
 				goto out;
@@ -204,7 +204,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 
 	if (devid != calibration_devid) {
 		mavlink_and_console_log_critical(mavlink_fd,
-						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
+						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		success = false;
 		goto out;
 	}

src/modules/commander/commander.cpp

@@ -553,7 +553,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				else {
 
 					//Refuse to arm if preflight checks have failed
-					if(!status.condition_system_sensors_initialized) {
+					if(!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
 						mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
 						cmd_result = VEHICLE_CMD_RESULT_DENIED;			
 						break;
@@ -1579,7 +1579,9 @@ int commander_thread_main(int argc, char *argv[])
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
+			if (armed.armed) {
+				mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
+			}
 			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
@@ -1587,7 +1589,6 @@ int commander_thread_main(int argc, char *argv[])
 			   && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
-			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (!armed.armed) {
@@ -1827,13 +1828,17 @@ int commander_thread_main(int argc, char *argv[])
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 &&
 			    hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
-				/* handle the case where data link was regained,
+				/* handle the case where data link was gained first time or regained,
 				 * accept datalink as healthy only after datalink_regain_timeout seconds
 				 * */
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					mavlink_log_info(mavlink_fd, "data link %i regained", i);
+					/* only report a regain */
+					if (telemetry_last_dl_loss[i] > 0) {
+						mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
+					}
+
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -1844,10 +1849,12 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-				telemetry_last_dl_loss[i]  = hrt_absolute_time();
 
 				if (!telemetry_lost[i]) {
-					mavlink_log_info(mavlink_fd, "data link %i lost", i);
+					/* only reset the timestamp to a different time on state change */
+					telemetry_last_dl_loss[i]  = hrt_absolute_time();
+
+					mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1862,7 +1869,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
+				mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;
@@ -2756,8 +2763,6 @@ void *commander_low_prio_loop(void *arg)
 				// Update preflight check status
 				status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
 
-				arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
-
 					if (((int)(cmd.param1)) == 0) {
 						int ret = param_load_default();
 
@@ -2781,9 +2786,15 @@ void *commander_low_prio_loop(void *arg)
 						}
 
 					} else if (((int)(cmd.param1)) == 1) {
+
 						int ret = param_save_default();
 
 						if (ret == OK) {
+							if (need_param_autosave) {
+								need_param_autosave = false;
+								need_param_autosave_timeout = 0;
+							}
+
 							mavlink_log_info(mavlink_fd, "settings saved");
 							answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 

src/modules/commander/state_machine_helper.cpp

@@ -137,6 +137,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		/* enforce lockdown in HIL */
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 			armed->lockdown = true;
+			prearm_ret = OK;
+			status->condition_system_sensors_initialized = true;
 
 		} else {
 			armed->lockdown = false;
@@ -174,7 +176,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 						// Fail transition if power is not good
 						if (!status->condition_power_input_valid) {
 
-							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
 							feedback_provided = true;
 							valid_transition = false;
 						}
@@ -184,7 +186,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
 							// Check avionics rail voltages
 							if (status->avionics_power_rail_voltage < 4.75f) {
-								mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+								mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
 								feedback_provided = true;
 								valid_transition = false;
 							} else if (status->avionics_power_rail_voltage < 4.9f) {
@@ -211,7 +213,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-			mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
+			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
 			feedback_provided = true;
 			valid_transition = false;
 			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
@@ -221,9 +223,9 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
 			new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 			if (status->condition_system_sensors_initialized) {
-				mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
 			} else {
-				mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
+				mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
 			}
 			feedback_provided = true;
 		}

src/modules/mavlink/mavlink_messages.cpp

@@ -539,7 +539,8 @@ protected:
 			msg.errors_count2 = status.errors_count2;
 			msg.errors_count3 = status.errors_count3;
 			msg.errors_count4 = status.errors_count4;
-			msg.battery_remaining = status.battery_remaining * 100.0f;
+			msg.battery_remaining = (msg.voltage_battery > 0) ?
+							status.battery_remaining * 100.0f : -1;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
 		}

src/modules/sdlog2/sdlog2.c

@@ -1648,10 +1648,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_VISN.vx = buf.vision_pos.vx;
 			log_msg.body.log_VISN.vy = buf.vision_pos.vy;
 			log_msg.body.log_VISN.vz = buf.vision_pos.vz;
-			log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
+			log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention
-			log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
+			log_msg.body.log_VISN.qx = buf.vision_pos.q[1];
-			log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
+			log_msg.body.log_VISN.qy = buf.vision_pos.q[2];
-			log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
+			log_msg.body.log_VISN.qz = buf.vision_pos.q[3];
 			LOGBUFFER_WRITE_AND_COUNT(VISN);
 		}
 

src/modules/uavcan/uavcan_main.cpp

@@ -698,7 +698,9 @@ int uavcan_main(int argc, char *argv[])
 
 	if (!std::strcmp(argv[1], "start")) {
 		if (UavcanNode::instance()) {
-			errx(1, "already started");
+			// Already running, no error
+			warnx("already started");
+			::exit(0);
 		}
 
 		// Node ID