INS: In indoor mode if GPS is found use that to populate PositionActual

flight/INS/ins.c

@@ -80,7 +80,6 @@ void zero_gyros(bool update_settings);
 //void send_calibration(void);
 void send_attitude(void);
 void send_velocity(void);
-void send_position(void);
 void homelocation_callback(AhrsObjHandle obj);
 //void calibration_callback(AhrsObjHandle obj);
 void settings_callback(AhrsObjHandle obj);
@@ -89,7 +88,7 @@ void calibration(float result[3], float scale[3][4], float arg[3]);
 
 extern void PIOS_Board_Init(void);
 void panic(uint32_t blinks);
-static void print_ekf_binary();
+static void print_ekf_binary(bool ekf);
 void simple_update();
 
 /* Bootloader related functions and var*/
@@ -256,9 +255,8 @@ int main()
 		
 		time_val2 = PIOS_DELAY_GetRaw();
 		
-		float zeros[3] = {0,0,0};
+		//print_ekf_binary(true);
-		INSSetGyroBias(zeros);
+
-		
 		switch(ahrs_algorithm) {
 			case INSSETTINGS_ALGORITHM_SIMPLE:
 				simple_update();
@@ -274,7 +272,7 @@ int main()
 				measure_noise();
 				break;
 			case INSSETTINGS_ALGORITHM_SENSORRAW:
-				print_ekf_binary();
+				print_ekf_binary(false);
 				// Run at standard rate
 				while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
 				break;
@@ -317,7 +315,7 @@ void simple_update() {
 /**
  * @brief Output all the important inputs and states of the ekf through serial port
  */
-static void print_ekf_binary()
+static void print_ekf_binary(bool ekf)
 {
 	static uint32_t timeval;
 	uint16_t delay;
@@ -337,9 +335,15 @@ static void print_ekf_binary()
 	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data.temperature, 4);
 	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data.temperature, 4);
 	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &delay, 2);
-	
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data));
-	mag_data.updated = 0;
+
-	altitude_data.updated = 0;
+	if(ekf)
+		PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Nav, sizeof(Nav));                                             // X (86:149)
+	else {
+		mag_data.updated = 0;
+		altitude_data.updated = 0;
+		gps_data.updated = 0;
+	}
 }
 
 void panic(uint32_t blinks) 
@@ -796,19 +800,6 @@ void send_velocity(void)
 	VelocityActualSet(&velocityActual);
 }
 
-void send_position(void)
-{
-	PositionActualData positionActual;
-	PositionActualGet(&positionActual);
-
-	// convert into cm
-	positionActual.North = Nav.Pos[0] * 100;
-	positionActual.East = Nav.Pos[1] * 100;
-	positionActual.Down = Nav.Pos[2] * 100;
-
-	PositionActualSet(&positionActual);
-}
-
 int callback_count = 0;
 void settings_callback(AhrsObjHandle obj)
 {

flight/INS/insgps_helper.c

@@ -79,10 +79,22 @@ void ins_outdoor_update()
 	attitude_data.quaternion.q3 = Nav.q[2];
 	attitude_data.quaternion.q4 = Nav.q[3];
 	send_attitude();  // get message out quickly
-	send_velocity();
-	send_position();
 	INSCovariancePrediction(dT);
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	positionActual.North = Nav.Pos[0] * 100;
+	positionActual.East = Nav.Pos[1] * 100;
+	positionActual.Down = Nav.Pos[2] * 100;
+	PositionActualSet(&positionActual);
 	
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+	velocityActual.North = Nav.Vel[0] * 100;
+	velocityActual.East = Nav.Vel[1] * 100;
+	velocityActual.Down = Nav.Vel[2] * 100;
+	VelocityActualSet(&velocityActual);
+
 	sensors = 0;
 	
 	/*
@@ -183,8 +195,6 @@ void ins_indoor_update()
 	attitude_data.quaternion.q3 = Nav.q[2];
 	attitude_data.quaternion.q4 = Nav.q[3];
 	send_attitude();  // get message out quickly
-	send_velocity();
-	send_position();
 	INSCovariancePrediction(dT);
 	
 	/* Indoors, update with zero position and velocity and high covariance */
@@ -221,6 +231,36 @@ void ins_indoor_update()
 		altitude_data.updated = false;
 	}
 	
+	if(gps_data.updated) {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		positionActual.North = gps_data.NED[0] * 100;
+		positionActual.East = gps_data.NED[1] * 100;
+		positionActual.Down = Nav.Pos[2] * 100;
+		PositionActualSet(&positionActual);
+		
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+		velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
+		velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
+		velocityActual.Down = Nav.Vel[2] * 100;
+		VelocityActualSet(&velocityActual);
+
+		gps_data.updated = false;
+	} else {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		positionActual.Down = Nav.Pos[2] * 100;
+		PositionActualSet(&positionActual);
+
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+		velocityActual.North = 0;
+		velocityActual.East = 0;
+		velocityActual.Down = Nav.Vel[2] * 100;
+		VelocityActualSet(&velocityActual);
+	}
+	
 	/*
 	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
 	 * although probably should occur within INS itself

flight/INS/pios_board.c

@@ -329,9 +329,9 @@ static const struct pios_usart_cfg pios_usart_gps_cfg = {
 	},
 };
 
-#define PIOS_COM_AUX_TX_BUF_LEN 255
+#define PIOS_COM_AUX_TX_BUF_LEN 512
 static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
-#define PIOS_COM_AUX_RX_BUF_LEN 255
+#define PIOS_COM_AUX_RX_BUF_LEN 512
 static uint8_t pios_com_aux_rx_buffer[PIOS_COM_AUX_RX_BUF_LEN];
 
 #endif /* PIOS_INCLUDE_GPS */