Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

apps/commander/commander.c

@@ -149,7 +149,6 @@ static void led_deinit(void);
 static int led_toggle(int led);
 static int led_on(int led);
 static int led_off(int led);
-static int pm_save_eeprom(bool only_unsaved);
 static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);

apps/commander/state_machine_helper.c

@@ -579,6 +579,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		state_machine_publish(status_pub, current_status, mavlink_fd);
 		publish_armed_status(current_status);
 		printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+	} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
+		mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
 	}
 
 	/* NEVER actually switch off HIL without reboot */

apps/drivers/hil/hil.cpp

@@ -474,6 +474,7 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
 		ret = HIL::pwm_ioctl(filp, cmd, arg);
 		break;
 	default:
+		ret = -ENOTTY;
 		debug("not in a PWM mode");
 		break;
 	}
@@ -489,7 +490,7 @@ int
 HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
+	// int channel;
 
 	lock();
 
@@ -824,6 +825,7 @@ hil_main(int argc, char *argv[])
 			// XXX all modes have PWM settings
 			if (argc > i + 1) {
 				pwm_update_rate_in_hz = atoi(argv[i + 1]);
+				printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
 			} else {
 				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
 				return 1;

apps/drivers/px4io/px4io.cpp

@@ -467,6 +467,7 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 
 	/* publish raw rc channel values from IO */
 	_input_rc.timestamp = hrt_absolute_time();
+	_input_rc.channel_count = rep->channel_count;
 	for (int i = 0; i < rep->channel_count; i++)
 	{
 		_input_rc.values[i] = rep->rc_channel[i];

apps/gps/mtk.c

@@ -418,10 +418,11 @@ void *mtk_watchdog_loop(void *args)
 				mtk_gps->satellite_info_available = 0;
 				// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
-				mtk_healthy = true;
-				mtk_fail_count = 0;
-				once_ok = true;
 			}
+
+			mtk_healthy = true;
+			mtk_fail_count = 0;
+			once_ok = true;
 		}
 
 		usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);

apps/gps/ubx.c

@@ -272,7 +272,8 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 
 						ubx_gps->timestamp = hrt_absolute_time();
 						ubx_gps->counter++;
-
+						ubx_gps->s_variance = packet->sAcc;
+						ubx_gps->p_variance = packet->pAcc;
 
 						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
@@ -785,10 +786,6 @@ void *ubx_watchdog_loop(void *args)
 			sleep(1);
 
 		} else {
-			/* gps healthy */
-			ubx_success_count++;
-			ubx_healthy = true;
-			ubx_fail_count = 0;
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
@@ -799,6 +796,10 @@ void *ubx_watchdog_loop(void *args)
 				once_ok = true;
 			}
 
+			/* gps healthy */
+			ubx_success_count++;
+			ubx_healthy = true;
+			ubx_fail_count = 0;
 		}
 
 		usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);

apps/mavlink/mavlink.c

@@ -163,8 +163,8 @@ set_hil_on_off(bool hil_enabled)
 			/* 20 Hz */
 			hil_rate_interval = 50;
 		} else {
-			/* 100 Hz */
-			hil_rate_interval = 10;
+			/* 200 Hz */
+			hil_rate_interval = 5;
 		}
 
 		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);

apps/sdlog/sdlog.c

@@ -488,23 +488,25 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
 		orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
 		orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
+		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
 
 		#pragma pack(push, 1)
 		struct {
-			uint64_t timestamp;
-			float gyro[3];
-			float accel[3];
-			float mag[3];
-			float baro;
-			float baro_alt;
-			float baro_temp;
-			float control[4];
-
-			float actuators[8];
-			float vbat;
-			float adc[3];
-			float local_pos[3];
-			int32_t gps_pos[3];
+			uint64_t timestamp; //[us]
+			float gyro[3]; //[rad/s]
+			float accel[3]; //[m/s^2]
+			float mag[3]; //[gauss]
+			float baro; //pressure [millibar]
+			float baro_alt; //altitude above MSL [meter]
+			float baro_temp; //[degree celcius]
+			float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+			float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
+			float vbat; //battery voltage in [volt]
+			float adc[3]; //remaining auxiliary ADC ports [volt]
+			float local_position[3]; //tangent plane mapping into x,y,z [m]
+			int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
+			float attitude[3]; //pitch, roll, yaw [rad]
+			float rotMatrix[9]; //unitvectors
 		} sysvector = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -518,14 +520,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
 					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
 			.vbat = buf.raw.battery_voltage_v,
 			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
-			.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
-			.gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}
+			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
+			.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
+			.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
+			.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
 		};
 		#pragma pack(pop)
 
 		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
 
-		usleep(10000);
+		usleep(10000);   //10000 corresponds in reality to ca. 76 Hz
 	}
 
 	fsync(sysvector_file);
@@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new)
    return ret;
 }
 
+

apps/sensors/sensors.cpp

@@ -93,6 +93,8 @@
 #define BAT_VOL_LOWPASS_2 0.01f
 #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
 
+#define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
+
 /**
  * Sensor app start / stop handling function
  *
@@ -865,8 +867,10 @@ Sensors::ppm_poll()
 	struct rc_input_values raw;
 
 	raw.timestamp = ppm_last_valid_decode;
+	/* we are accepting this message */
+	_ppm_last_valid = ppm_last_valid_decode;
 
-	if (ppm_decoded_channels > 1) {
+	if (ppm_decoded_channels > 2 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
 
 		for (int i = 0; i < ppm_decoded_channels; i++) {
 			raw.values[i] = ppm_buffer[i];

apps/systemlib/perf_counter.c

@@ -73,6 +73,20 @@ struct perf_ctr_elapsed {
 	uint64_t		time_most;
 };
 
+/**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint64_t		time_least;
+	uint64_t		time_most;
+
+};
+
 /**
  * List of all known counters.
  */
@@ -93,6 +107,10 @@ perf_alloc(enum perf_counter_type type, const char *name)
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
 		break;
 
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+		break;
+
 	default:
 		break;
 	}
@@ -127,6 +145,32 @@ perf_count(perf_counter_t handle)
 		((struct perf_ctr_count *)handle)->event_count++;
 		break;
 
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		hrt_abstime now = hrt_absolute_time();
+
+		switch (pci->event_count) {
+		case 0:
+			pci->time_first = now;
+			break;
+		case 1:
+			pci->time_least = now - pci->time_last;
+			pci->time_most = now - pci->time_last;
+			break;
+		default: {
+			hrt_abstime interval = now - pci->time_last;
+			if (interval < pci->time_least)
+				pci->time_least = interval;
+			if (interval > pci->time_most)
+				pci->time_most = interval;
+			break;
+		}
+		}
+		pci->time_last = now;
+		pci->event_count++;
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -187,13 +231,29 @@ perf_print_counter(perf_counter_t handle)
 		       ((struct perf_ctr_count *)handle)->event_count);
 		break;
 
-	case PC_ELAPSED:
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
 		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
 		       handle->name,
-		       ((struct perf_ctr_elapsed *)handle)->event_count,
-		       ((struct perf_ctr_elapsed *)handle)->time_total,
-		       ((struct perf_ctr_elapsed *)handle)->time_least,
-		       ((struct perf_ctr_elapsed *)handle)->time_most);
+		       pce->event_count,
+		       pce->time_total,
+		       pce->time_least,
+		       pce->time_most);
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		       handle->name,
+		       pci->event_count,
+		       (pci->time_last - pci->time_first) / pci->event_count,
+		       pci->time_least,
+		       pci->time_most);
+		break;
+	}
 
 	default:
 		break;

apps/systemlib/perf_counter.h

@@ -44,7 +44,8 @@
  */
 enum perf_counter_type {
 	PC_COUNT,		/**< count the number of times an event occurs */
-	PC_ELAPSED		/**< measure the time elapsed performing an event */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
 };
 
 struct perf_ctr_header;

apps/uORB/topics/vehicle_gps_position.h

@@ -65,6 +65,8 @@ struct vehicle_gps_position_s
 	uint16_t counter_pos_valid;		/**< is only increased when new lat/lon/alt information was added   */
 	uint16_t eph;				/**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME   */
 	uint16_t epv;				/**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535   */
+	float s_variance;			// XXX testing
+	float p_variance;			// XXX testing
 	uint16_t vel;				/**< GPS ground speed (m/s * 100). If unknown, set to: 65535   */
 	float vel_n;				/**< GPS ground speed in m/s */
 	float vel_e;				/**< GPS ground speed in m/s */