support qmc5883

Author: xiaosanti

Makefile

@@ -152,7 +152,8 @@ endif
 export UAVOBJGENERATOR
 
 # Set up default build configurations (debug | release)
-GCS_BUILD_CONF := release
+# GCS_BUILD_CONF := release
+GCS_BUILD_CONF := debug
 
 # Set extra configuration
 ifeq ($(GCS_WITH_OSG), 1)

flight/make/apps-defs.mk

@@ -105,6 +105,7 @@ SRC += $(PIOSCOMMON)/pios_openlrs.c
 SRC += $(PIOSCOMMON)/pios_openlrs_rcvr.c
 SRC += $(PIOSCOMMON)/pios_board_io.c
 SRC += $(PIOSCOMMON)/pios_board_sensors.c
+SRC += $(PIOSCOMMON)/pios_qmc5883.c
 
 ## Misc library functions
 SRC += $(FLIGHTLIB)/sanitycheck.c

flight/modules/Sensors/sensors.c

@@ -87,6 +87,8 @@
 
 static const TickType_t sensor_period_ticks = ((uint32_t)(1000.0f / PIOS_SENSOR_RATE / (float)portTICK_RATE_MS));
 
+// PIOS_SENSOR_RATE 500 HZ
+
 // Interval in number of sample to recalculate temp bias
 #define TEMP_CALIB_INTERVAL      30
 
@@ -310,7 +312,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
             // we will wait on the sensor that's marked as primary( that means the sensor with higher sample rate)
             bool is_primary = (sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL);
 
-            if (!sensor->driver->is_polled) {
+            if (!sensor->driver->is_polled) { // is_polled false mpu6k, qmc5883
                 const QueueHandle_t queue = PIOS_SENSORS_GetQueue(sensor);
                 while (xQueueReceive(queue,
                                      (void *)source_data,
@@ -326,7 +328,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters)
                     PERF_TRACK_VALUE(counterSensorResets, reset_counter);
                     error = true;
                 }
-            } else {
+            } else { // poll manually
                 if (PIOS_SENSORS_Poll(sensor)) {
                     PIOS_SENSOR_Fetch(sensor, (void *)source_data, MAX_SENSORS_PER_INSTANCE);
                     if (sensor->type & PIOS_SENSORS_TYPE_3D) {
@@ -494,6 +496,18 @@ static void handleMag(float *samples, float temperature)
 
     rot_mult(mag_transform, mags, samples);
 
+	/*
+	// Debug
+	static uint32_t debug_ticks_pre = 0;
+	static uint32_t debug_ticks_now = 0;
+	debug_ticks_now++;
+	if (debug_ticks_now >= debug_ticks_pre + 2500) {
+	    debug_ticks_pre = 0;
+		debug_ticks_now = 0;
+        DEBUG_PRINTF(0, "handle mag: %f,%f,%f\r\n", mags[0],mags[1],mags[2]);
+	}
+	*/
+
     mag.x = samples[0];
     mag.y = samples[1];
     mag.z = samples[2];

flight/pios/common/pios_board_sensors.c

@@ -53,6 +53,13 @@
 # include <pios_mpu9250_config.h>
 #endif
 
+#ifdef PIOS_INCLUDE_QMC5883
+#include "pios_qmc5883.h"
+# ifdef PIOS_QMC5883_HAS_GPIOS
+pios_qmc5883_dev_t pios_qmc5883_internal_id;
+# endif
+#endif
+
 #ifdef PIOS_INCLUDE_HMC5X83
 # include "pios_hmc5x83.h"
 # ifdef PIOS_HMC5X83_HAS_GPIOS
@@ -123,6 +130,69 @@ void PIOS_BOARD_Sensors_Configure()
     }
 #endif
 
+// internal QMC5883 mag
+# ifdef PIOS_INCLUDE_QMC5883_INTERNAL
+    const struct pios_qmc5883_cfg *qmc5883_internal_cfg = PIOS_BOARD_HW_DEFS_GetInternalQMC5883Cfg(pios_board_info_blob.board_rev);
+
+    if (qmc5883_internal_cfg) {
+        // attach the 5883 mag to internal i2c bus
+
+        pios_qmc5883_dev_t internal_mag = PIOS_QMC5883_Init(qmc5883_internal_cfg, PIOS_I2C_QMC5883_INTERNAL_ADAPTER, 0);
+#  ifdef PIOS_INCLUDE_WDG
+        // give QMC5883 on I2C some extra time to allow for reset, etc. if needed
+        // this is not in a loop, so it is safe
+        PIOS_WDG_Clear();
+#  endif /* PIOS_INCLUDE_WDG */
+
+#ifdef PIOS_QMC5883_HAS_GPIOS
+        pios_qmc5883_internal_id = internal_mag;
+#endif
+        // add this sensor to the sensor task's list
+        PIOS_QMC5883_Register(internal_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
+    }
+
+# endif /* PIOS_INCLUDE_QMC5883_INTERNAL */
+
+# ifdef PIOS_INCLUDE_QMC5883
+
+    AuxMagSettingsTypeOptions option;
+    AuxMagSettingsTypeGet(&option);
+
+    const struct pios_qmc5883_cfg *qmc5883_external_cfg = PIOS_BOARD_HW_DEFS_GetExternalQMC5883Cfg(pios_board_info_blob.board_rev);
+
+    if (qmc5883_external_cfg) {
+        uint32_t i2c_id = 0;
+
+        if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
+            // i2c_external
+#ifdef PIOS_I2C_FLEXI_ADAPTER
+            i2c_id = PIOS_I2C_FLEXI_ADAPTER;
+#endif
+        } else if (option == AUXMAGSETTINGS_TYPE_I2C) {
+            // i2c_internal (or Sparky2/F3 dedicated I2C port)
+#ifdef PIOS_I2C_EXTERNAL_ADAPTER
+            i2c_id = PIOS_I2C_EXTERNAL_ADAPTER;
+#endif
+        }
+
+        if (i2c_id) {
+            uint32_t external_mag = PIOS_QMC5883_Init(qmc5883_external_cfg, i2c_id, 0);
+#  ifdef PIOS_INCLUDE_WDG
+            // give QMC5883 on I2C some extra time to allow for reset, etc. if needed
+            // this is not in a loop, so it is safe
+            PIOS_WDG_Clear();
+#  endif /* PIOS_INCLUDE_WDG */
+            // add this sensor to the sensor task's list
+            // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
+            // and before registering some other fast and important sensor
+            // as that would cause delay and time jitter for the second fast sensor
+            PIOS_QMC5883_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
+            // mag alarm is cleared later, so use I2C
+            AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
+        }
+    }
+# endif /* PIOS_INCLUDE_QMC5883 */
+
     // internal HMC5x83 mag
 # ifdef PIOS_INCLUDE_HMC5X83_INTERNAL
     const struct pios_hmc5x83_cfg *hmc5x83_internal_cfg = PIOS_BOARD_HW_DEFS_GetInternalHMC5x83Cfg(pios_board_info_blob.board_rev);

flight/pios/common/pios_hmc5x83.c

@@ -149,7 +149,7 @@ pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_
             break;
         }
         // if the application sensor rate is fast enough to warrant skipping some slow hardware sensor reads
-        if ((PIOS_SENSOR_RATE * 100.0f / 3.0f) > rate100) {
+        if ((PIOS_SENSOR_RATE * 100.0f / 3.0f/*3-AXIS*/) > rate100) {
             // count the number of "return false" up to this number
             dev->magCountMax = ((uint16_t)PIOS_SENSOR_RATE * 100 / rate100) + 1;
         } else {