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

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/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();
 

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/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/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;