Airspeed sensor scale

Tools/sdlog2/sdlog2_dump.py

@@ -154,8 +154,8 @@ def process(self, fn):
                         first_data_msg = False
                     self.__parseMsg(msg_descr)
             bytes_read += self.__ptr
-        if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
-            self.__printCSVRow()
+            if not self.__debug_out and self.__time_msg != None and self.__csv_updated:
+                self.__printCSVRow()
         f.close()
 
     def __bytesLeft(self):

makefiles/config_px4fmu-v1_APM.mk

@@ -0,0 +1,83 @@
+#
+# Makefile for the px4fmu-v1_APM configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(SKETCHBOOK)/mk/PX4/ROMFS
+
+MODULES		+= $(APM_MODULE_DIR)
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4io
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/boards/px4fmu-v1
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/l3gd20
+# MODULES		+= drivers/bma180
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+MODULES		+= drivers/ll40ls
+MODULES		+= drivers/gps
+#MODULES		+= drivers/hil
+#MODULES		+= drivers/hott_telemetry
+MODULES		+= drivers/blinkm
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= drivers/mkblctrl
+#MODULES	+= modules/sensors
+
+#
+# System commands
+#
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/i2c
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/nshterm
+MODULES         += systemcmds/auth
+
+#
+# Libraries
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/uORB
+MODULES		+= lib/mathlib/math/filter
+MODULES	        += modules/libtomfastmath
+MODULES         += modules/libtomcrypt
+MODULES		+= lib/conversion
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                )

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=2048
+CONFIG_SCHED_WORKSTACKSIZE=4000
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=2048
+CONFIG_SCHED_LPWORKSTACKSIZE=4000
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -795,11 +795,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=2000
+CONFIG_SCHED_WORKSTACKSIZE=4000
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=2000
+CONFIG_SCHED_LPWORKSTACKSIZE=4000
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

src/drivers/drv_baro.h

@@ -58,6 +58,10 @@ struct baro_report {
 	float temperature;
 	uint64_t timestamp;
 	uint64_t error_count;
+
+	// raw MS5611 values for debugging
+	uint32_t ms5611_D1;
+	uint32_t ms5611_D2;
 };
 
 /*

src/drivers/drv_mag.h

@@ -83,6 +83,13 @@ ORB_DECLARE(sensor_mag0);
 ORB_DECLARE(sensor_mag1);
 ORB_DECLARE(sensor_mag2);
 
+/*
+ * mag device types, for _device_id
+ */
+#define DRV_MAG_DEVTYPE_HMC5883 1
+#define DRV_MAG_DEVTYPE_LSM303D 2
+
+
 /*
  * mag device types, for _device_id
  */

src/drivers/drv_pwm_output.h

@@ -213,6 +213,12 @@ ORB_DECLARE(output_pwm);
 /** make failsafe non-recoverable (termination) if it occurs */
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+
+/** force safety switch on (to enable use of safety switch) */
+#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 25)
+
 /*
  *
  *

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -88,6 +88,8 @@
  */
 #define MIN_ACCURATE_DIFF_PRES_PA 0
 
+#define DIFF_PRES_SCALE_ETS 1 /* Default pressure scaling */
+
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
@@ -106,6 +108,11 @@ class ETSAirspeed : public Airspeed
 	virtual int	measure();
 	virtual int	collect();
 
+	/**
+	* Get the default scaling for this sensor
+	*/
+	virtual float	get_default_scale();
+
 };
 
 /*
@@ -164,6 +171,10 @@ ETSAirspeed::collect()
 		log("zero value from sensor"); 
 		return -1;
         }
+
+	// the ETS sensor outputs Pa directly, so scaling is never applied
+	// to its output. 
+	float diff_pres_pa = (float) diff_pres_pa_raw;
 
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
@@ -256,6 +267,16 @@ ETSAirspeed::cycle()
 		   USEC2TICK(CONVERSION_INTERVAL));
 }
 
+
+/**
+* get default scale of the sensor
+*/
+float
+ETSAirspeed::get_default_scale() 
+{
+	return DIFF_PRES_SCALE_ETS;
+}
+
 /**
  * Local functions in support of the shell command.
  */

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -373,6 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	if (voltage_diff < -1.0f) {
 		voltage_diff = -1.0f;
 	}
+	temperature -= voltage_diff * temp_slope;	
 	temperature -= voltage_diff * temp_slope;
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }

src/drivers/mpu6000/mpu6000.cpp

@@ -174,7 +174,7 @@
   SPI speed
  */
 #define MPU6000_LOW_BUS_SPEED				1000*1000
-#define MPU6000_HIGH_BUS_SPEED				11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
+#define MPU6000_HIGH_BUS_SPEED				10*1000*1000
 
 class MPU6000_gyro;
 
@@ -229,6 +229,7 @@ class MPU6000 : public device::SPI
 	perf_counter_t		_gyro_reads;
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_bad_transfers;
+	perf_counter_t		_good_transfers;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
@@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
 	_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
+	_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
 	_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -456,6 +458,7 @@ MPU6000::~MPU6000()
 	perf_free(_accel_reads);
 	perf_free(_gyro_reads);
 	perf_free(_bad_transfers);
+	perf_free(_good_transfers);
 }
 
 int
@@ -1279,8 +1282,14 @@ MPU6000::measure()
 		// all zero data - probably a SPI bus error
 		perf_count(_bad_transfers);
 		perf_end(_sample_perf);
+                // note that we don't call reset() here as a reset()
+                // costs 20ms with interrupts disabled. That means if
+                // the mpu6k does go bad it would cause a FMU failure,
+                // regardless of whether another sensor is available,
 		return;
 	}
+
+	perf_count(_good_transfers);
 
 
 	/*
@@ -1399,6 +1408,8 @@ MPU6000::print_info()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_accel_reads);
 	perf_print_counter(_gyro_reads);
+	perf_print_counter(_bad_transfers);
+	perf_print_counter(_good_transfers);
 	_accel_reports->print_info("accel queue");
 	_gyro_reports->print_info("gyro queue");
 }

src/drivers/ms5611/ms5611_spi.cpp

@@ -129,7 +129,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
-	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 6*1000*1000),
 	_prom(prom_buf)
 {
 }

src/drivers/px4fmu/fmu.cpp

@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
 	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		// these are no-ops, as no safety switch
 		break;
 

src/drivers/px4io/px4io.cpp

@@ -106,6 +106,15 @@ extern device::Device *PX4IO_serial_interface() weak_function;
 #define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
 #define IO_POLL_INTERVAL		20000		// 20 ms -> 50 Hz
 
+#ifndef ARDUPILOT_BUILD
+# define RC_HANDLING_DEFAULT false
+#else
+// APM uses raw PWM output
+# define RC_HANDLING_DEFAULT true
+// APM doesn't yet use a mavlink log listener
+# define mavlink_vasprintf(_fd, severity, fmt, ...)
+#endif
+
 /**
  * The PX4IO class.
  *
@@ -485,7 +494,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_max_relays(0),
 	_max_transfer(16),	/* sensible default */
 	_update_interval(0),
-	_rc_handling_disabled(false),
+	_rc_handling_disabled(RC_HANDLING_DEFAULT),
 	_rc_chan_count(0),
 	_rc_last_valid(0),
 	_task(-1),
@@ -2278,6 +2287,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_SAFETY_ON:
+		/* force safety switch on */
+		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
+		break;
+
 	case PWM_SERVO_SET_FORCE_FAILSAFE:
 		/* force failsafe mode instantly */
 		if (arg == 0) {