AHRS: Added message to pass GPS information into AHRS from OP. This is then converted

into NED reference frame and used in the INSGPS algorithm, although currently this
information isn't propagated back to OP.  Data structures related to the GPS position
into the algorithm and the position estimate out will likely be in flux.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1334 ebee16cc-31ac-478f-84a7-5cbb03baadba

flight/AHRS/ahrs.c

@@ -149,23 +149,22 @@ struct altitude_sensor {
   float temperature;
 };
 
+struct gps_sensor {
+  float latitude;
+  float longitude;
+  float altitude;
+  float heading;
+  float groundspeed;
+  float status;
+};
+
 static struct mag_sensor        mag_data;
 static struct accel_sensor      accel_data;
 static struct gyro_sensor       gyro_data;
 static struct altitude_sensor   altitude_data;
-static struct attitude_solution attitude_data = {
+static struct gps_sensor        gps_data;
-  .quaternion = {
+static struct attitude_solution attitude_data;
-    .q1 = 1.011,
+
-    .q2 = 2.022,
-    .q3 = 3.033,
-    .q4 = 0,
-  },
-  .euler = {
-    .roll  = 4.044,
-    .pitch = 5.055,
-    .yaw   = 6.066,
-  },
-};
 /**
  * @}
  */
@@ -175,6 +174,7 @@ static struct attitude_solution attitude_data = {
 void process_spi_request(void);
 void downsample_data(void);
 void calibrate_sensors(void);
+void initialize_location();
 
 /**
  * @addtogroup AHRS_Global_Data AHRS Global Data
@@ -203,6 +203,16 @@ int16_t * valid_data_buffer;
 uint32_t ekf_too_slow = 0;
 //! Total number of data blocks converted
 uint32_t total_conversion_blocks = 0;
+//! Flag to indicate new GPS data available
+uint8_t gps_updated = FALSE;
+//! Home location in ECEF coordinates
+double BaseECEF[3] = {0, 0, 0};
+//! Rotation matrix from LLA to Rne
+float Rne[3][3];
+//! Current location in NED coordinates relative to base
+float NED[3];
+//! Current location in NED coordinates relative to base
+
 /**
  * @}
  */
@@ -215,7 +225,7 @@ int main()
 {
   float gyro[3], accel[3], mag[3];
   float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0;
-  uint32_t update_ctr = 0;
+  uint32_t loop_ctr = 0;
   ahrs_algorithm = INSGPS_Algo;
   
   /* Brings up System using CMSIS functions, enables the LEDs. */
@@ -281,7 +291,7 @@ int main()
     float scaled_mag_var[3] = {mag_var[0] / mag_length, mag_var[1] / mag_length, mag_var[2] / mag_length};
     INSSetMagVar(scaled_mag_var);     
     
-/*    // Initialize the algorithm assuming stationary    
+    // Initialize the algorithm assuming stationary    
     float temp_var[3] = {10, 10, 10};
     float temp_gyro[3] = {0, 0, 0};
 
@@ -297,7 +307,7 @@ int main()
       RPY2Quaternion(rpy,Nav.q);
       INSPrediction( temp_gyro, accel_bias, 1 / (float) EKF_RATE );
       FullCorrection(mag,pos,vel,BaroAlt); 
-    }    */
+    }    
 
     INSSetGyroBias(gyro_bias);
     INSSetAccelVar(accel_var);
@@ -321,6 +331,8 @@ int main()
     // Alive signal
     PIOS_LED_Toggle(LED1);
     
+    loop_ctr ++;
+    
     // Get magnetic readings
     PIOS_HMC5843_ReadMag(mag_data.raw.axis);
     
@@ -364,11 +376,35 @@ int main()
       mag[2] = -mag_data.raw.axis[2];
             
       INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
-      update_ctr++;
+
-      if ( 0 )
+      if ( ((loop_ctr % 10000) == 0) && (gps_data.status > 0) )
-        MagCorrection(mag);
+      {
-      else if ( 1 );
+        // cheap logic instead of detecting the first update of gps data
-        FullCorrection(mag,pos,vel,BaroAlt); 
+        WMM_GetMagVector(gps_data.latitude, gps_data.longitude, gps_data.altitude, 8, 17, 2010, MagNorth);
+        float MagNorthLen = sqrt(MagNorth[0] * MagNorth[0] + MagNorth[1] * MagNorth[1] + MagNorth[2] * MagNorth[2]);
+        float MagNorthScaled[3] = {MagNorth[0] / MagNorthLen, MagNorth[1] / MagNorthLen, MagNorth[2] / MagNorthLen};
+        INSSetMagNorth(MagNorthScaled);
+      }  
+      
+      if ( gps_updated ) 
+      {
+        if(BaseECEF[0] == 0 && BaseECEF[1] == 0 && BaseECEF[2] == 0)
+          initialize_location();
+        
+        // Convert to NED frame.  Probably this will be moved to OP mainboard
+        double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
+        LLA2Base(LLA, BaseECEF, Rne, NED);
+
+        // Compute velocity from Heading and groundspeed
+        vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);        
+        vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
+        vel[2] = 0;
+        FullCorrection(mag, NED, vel, altitude_data.altitude);
+
+        gps_updated = FALSE;
+      }
+      else
+        MagCorrection(mag); 
       
       Quaternion2RPY(Nav.q,rpy);
       attitude_data.quaternion.q1 = Nav.q[0];
@@ -407,6 +443,18 @@ int main()
   return 0;
 }
 
+/**
+ * @brief Initialize a location as the home point and compute rotation matrix to
+ * get to NED coordinates
+ */
+void initialize_location() 
+{
+  double LLA[3] = {gps_data.latitude, gps_data.longitude, gps_data.altitude};
+  RneFromLLA(LLA, Rne);
+  LLA2ECEF(LLA ,BaseECEF);
+}
+
+
 /**
  * @brief Downsample the analog data
  * @return none
@@ -629,6 +677,18 @@ void process_spi_request(void)
     dump_spi_message(PIOS_COM_AUX, "V", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
     lfsm_user_set_tx_v1 (&user_tx_v1);
     break;
+  case OPAHRS_MSG_V1_REQ_GPS:
+    opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_GPS);
+    gps_updated = TRUE;
+    gps_data.latitude    = user_rx_v1.payload.user.v.req.gps.latitude;
+    gps_data.longitude    = user_rx_v1.payload.user.v.req.gps.longitude;
+    gps_data.altitude = user_rx_v1.payload.user.v.req.gps.altitude;
+    gps_data.heading = user_rx_v1.payload.user.v.req.gps.heading;
+    gps_data.groundspeed = user_rx_v1.payload.user.v.req.gps.groundspeed;
+    gps_data.status = user_rx_v1.payload.user.v.req.gps.status;
+    dump_spi_message(PIOS_COM_AUX, "V", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
+    lfsm_user_set_tx_v1 (&user_tx_v1);
+    break;
   case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
     opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_ATTITUDERAW);
     user_tx_v1.payload.user.v.rsp.attituderaw.mags.x        = mag_data.raw.axis[0];

flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c

@@ -58,6 +58,7 @@
 #include "alarms.h"
 #include "altitudeactual.h"
 #include "stdbool.h"
+#include "positionactual.h"
 
 #include "pios_opahrs.h" // library for OpenPilot AHRS access functions
 #include "pios_opahrs_proto.h"
@@ -74,6 +75,7 @@ static xTaskHandle taskHandle;
 // Private functions
 static void ahrscommsTask(void* parameters);
 static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude);
+static void load_position_actual(struct opahrs_msg_v1_req_gps * gps);
 static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude);
 static void update_attitude_raw(struct opahrs_msg_v1_rsp_attituderaw * attituderaw);
 static void update_ahrs_status(struct opahrs_msg_v1_rsp_serial * serial);
@@ -84,6 +86,12 @@ static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
   AltitudeActualIsUpdatedFlag = true;
 }
 
+static bool PositionActualIsUpdatedFlag = false;
+static void PositionActualUpdatedCb(UAVObjEvent * ev)
+{
+	PositionActualIsUpdatedFlag = true;
+}
+
 /**
  * Initialise the module, called on startup
  * \returns 0 on success or -1 if initialisation failed
@@ -91,6 +99,7 @@ static void AltitudeActualUpdatedCb(UAVObjEvent * ev)
 int32_t AHRSCommsInitialize(void)
 {
   AltitudeActualConnectCallback(AltitudeActualUpdatedCb);
+  PositionActualConnectCallback(PositionActualUpdatedCb);
 
   PIOS_OPAHRS_Init();
 
@@ -136,30 +145,41 @@ static void ahrscommsTask(void* parameters)
       AttitudeSettingsGet(&settings);
   
       if (PIOS_OPAHRS_GetAttitude(&rsp) == OPAHRS_RESULT_OK) {
-	update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
+        update_attitude_actual(&(rsp.payload.user.v.rsp.attitude));
       } else {
-	/* Comms error */
+        /* Comms error */
-	break;
+        break;
       }
 
       if (PIOS_OPAHRS_GetAttitudeRaw(&rsp) == OPAHRS_RESULT_OK) {
-	update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
+	      update_attitude_raw(&(rsp.payload.user.v.rsp.attituderaw));
       } else {
-	/* Comms error */
+        /* Comms error */
-	break;
+        break;
       }
 
       if (AltitudeActualIsUpdatedFlag) {
-	struct opahrs_msg_v1 req;
+        struct opahrs_msg_v1 req;
 
-	load_altitude_actual(&(req.payload.user.v.req.altitude));
+        load_altitude_actual(&(req.payload.user.v.req.altitude));
-	if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
+        if (PIOS_OPAHRS_SetAltitudeActual(&req) == OPAHRS_RESULT_OK) {
-	  AltitudeActualIsUpdatedFlag = false;
+          AltitudeActualIsUpdatedFlag = false;
-	} else {
+        } else {
-	  /* Comms error */
+          /* Comms error */
-	}
+        }
       }
 
+      if (PositionActualIsUpdatedFlag) {
+        struct opahrs_msg_v1 req;
+        
+        load_position_actual(&(req.payload.user.v.req.gps));
+        if (PIOS_OPAHRS_SetPositionActual(&req) == OPAHRS_RESULT_OK) {
+          PositionActualIsUpdatedFlag = false;
+        } else {
+          /* Comms error */
+        }
+      }
+      
       /* Wait for the next update interval */
       vTaskDelay( settings.UpdatePeriod / portTICK_RATE_MS );
     }
@@ -177,6 +197,19 @@ static void load_altitude_actual(struct opahrs_msg_v1_req_altitude * altitude)
   altitude->temperature = data.Temperature;
 }
 
+static void load_position_actual(struct opahrs_msg_v1_req_gps * gps)
+{
+  PositionActualData data;
+  PositionActualGet(&data);
+  
+  gps->latitude = data.Latitude;
+	gps->longitude = data.Longitude;
+	gps->altitude = data.GeoidSeparation;
+  gps->heading = data.Heading;
+  gps->groundspeed = data.Groundspeed;
+  gps->status = data.Status;
+}
+
 static void update_attitude_actual(struct opahrs_msg_v1_rsp_attitude * attitude)
 {
   AttitudeActualData   data;

flight/PiOS/Common/pios_opahrs.c

@@ -266,6 +266,28 @@ enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp)
   return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_ATTITUDE, rsp);
 }
 
+enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req)
+{
+  struct opahrs_msg_v1 rsp;
+  enum opahrs_result   rc;
+  
+  if (!req) {
+    return -1;
+  }
+  
+  /* Make up an attituderaw request */
+  opahrs_msg_v1_init_user_tx (req, OPAHRS_MSG_V1_REQ_GPS);
+  
+  /* Send the message until it is received */
+  rc = opahrs_msg_v1_send_req (req);
+  if (rc != OPAHRS_RESULT_OK) {
+    /* Failed to send the request, bail out */
+    return rc;
+  }
+  
+  return opahrs_msg_v1_recv_rsp (OPAHRS_MSG_V1_RSP_GPS, &rsp);
+}
+
 enum opahrs_result PIOS_OPAHRS_SetAltitudeActual(struct opahrs_msg_v1 *req)
 {
   struct opahrs_msg_v1 rsp;

flight/PiOS/inc/pios_opahrs.h

@@ -44,6 +44,7 @@ extern enum opahrs_result PIOS_OPAHRS_GetSerial(struct opahrs_msg_v1 *rsp);
 extern enum opahrs_result PIOS_OPAHRS_GetAttitude(struct opahrs_msg_v1 *rsp);
 extern enum opahrs_result PIOS_OPAHRS_GetAttitudeRaw(struct opahrs_msg_v1 *rsp);
 extern enum opahrs_result PIOS_OPAHRS_SetAltitudeActual(struct opahrs_msg_v1 *req);
+extern enum opahrs_result PIOS_OPAHRS_SetPositionActual(struct opahrs_msg_v1 *req);
 extern enum opahrs_result PIOS_OPAHRS_resync(void);
 
 #endif /* PIOS_OPAHRS_H */

flight/PiOS/inc/pios_opahrs_proto.h

@@ -190,6 +190,15 @@ struct opahrs_msg_v1_req_altitude {
   float temperature;
 } __attribute__((__packed__));
 
+struct opahrs_msg_v1_req_gps {
+	float latitude;
+	float longitude;
+	float altitude;
+	float heading;
+	float groundspeed;
+	uint8_t status;
+} __attribute__((__packed__));
+
 struct opahrs_msg_v1_req_attituderaw {
 } __attribute__((__packed__));
 
@@ -203,6 +212,7 @@ union opahrs_msg_v1_req {
   struct opahrs_msg_v1_req_reset        reset;
   struct opahrs_msg_v1_req_serial       serial;
   struct opahrs_msg_v1_req_altitude     altitude;
+  struct opahrs_msg_v1_req_gps          gps;
   struct opahrs_msg_v1_req_attituderaw  attituderaw;
   struct opahrs_msg_v1_req_attitude     attitude;
 } __attribute__((__packed__));
@@ -223,6 +233,9 @@ struct opahrs_msg_v1_rsp_serial {
 struct opahrs_msg_v1_rsp_altitude {
 } __attribute__((__packed__));
 
+struct opahrs_msg_v1_rsp_gps {
+} __attribute__((__packed__));
+
 struct opahrs_msg_v1_rsp_attituderaw {
   struct {
     int16_t x;
@@ -273,6 +286,7 @@ union opahrs_msg_v1_rsp {
   struct opahrs_msg_v1_rsp_altitude     altitude;
   struct opahrs_msg_v1_rsp_attituderaw  attituderaw;
   struct opahrs_msg_v1_rsp_attitude     attitude;
+  struct opahrs_msg_v1_rsp_altitude     gps;
 } __attribute__((__packed__));
 
 enum opahrs_msg_v1_tag {
@@ -281,12 +295,14 @@ enum opahrs_msg_v1_tag {
   OPAHRS_MSG_V1_REQ_RESET,
   OPAHRS_MSG_V1_REQ_SERIAL,
   OPAHRS_MSG_V1_REQ_ALTITUDE,
+  OPAHRS_MSG_V1_REQ_GPS,
   OPAHRS_MSG_V1_REQ_ATTITUDERAW,
   OPAHRS_MSG_V1_REQ_ATTITUDE,
 
   OPAHRS_MSG_V1_RSP_SYNC,
   OPAHRS_MSG_V1_RSP_SERIAL,
   OPAHRS_MSG_V1_RSP_ALTITUDE,
+  OPAHRS_MSG_V1_RSP_GPS,
   OPAHRS_MSG_V1_RSP_ATTITUDERAW,
   OPAHRS_MSG_V1_RSP_ATTITUDE,
 };

flight/Project/OpenOCD/floss-jtag.ahrs.osx.cfg

@@ -10,6 +10,6 @@ ft2232_vid_pid 0x0403 0x6010
 ft2232_layout "usbjtag"
 ft2232_latency 2
 
-gdb_port 3333
+gdb_port 3334
 tcl_port 6666
 telnet_port 4444