Пример #1
0
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	char * function_name;
	float accel_data[3];
	float gyro_data[3];
	float mag_data[3];
	float pos_data[3];
	float vel_data[3];
	float baro_data;
	float dT;

	//All code and internal function calls go in here!
	if(!mxIsChar(prhs[0])) {
		mexErrMsgTxt("First parameter must be name of a function\n");
		return;
	} 

	if(mlStringCompare(prhs[0], "INSGPSInit")) {
		INSGPSInit();
	} else 	if(mlStringCompare(prhs[0], "INSStatePrediction")) {

		if(nrhs != 4) {
			mexErrMsgTxt("Incorrect number of inputs for state prediction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], gyro_data, 3) || 
			!mlGetFloatArray(prhs[2], accel_data, 3) ||
			!mlGetFloatArray(prhs[3], &dT, 1)) 
			return;

		INSStatePrediction(gyro_data, accel_data, dT);
		INSCovariancePrediction(dT);
	} else 	if(mlStringCompare(prhs[0], "INSFullCorrection")) {

		if(nrhs != 5) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], mag_data, 3) ||
				!mlGetFloatArray(prhs[2], pos_data, 3) ||
				!mlGetFloatArray(prhs[3], vel_data ,3) ||
				!mlGetFloatArray(prhs[4], &baro_data, 1)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		FullCorrection(mag_data, pos_data, vel_data, baro_data);
	} else 	if(mlStringCompare(prhs[0], "INSMagCorrection")) {
		if(nrhs != 2) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], mag_data, 3)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		MagCorrection(mag_data);
    } else 	if(mlStringCompare(prhs[0], "INSBaroCorrection")) {
		if(nrhs != 2) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], &baro_data, 1)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		BaroCorrection(baro_data);
	} else 	if(mlStringCompare(prhs[0], "INSMagVelBaroCorrection")) {

		if(nrhs != 4) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], mag_data, 3) ||
				!mlGetFloatArray(prhs[2], vel_data ,3) ||
				!mlGetFloatArray(prhs[3], &baro_data, 1)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		MagVelBaroCorrection(mag_data, vel_data, baro_data);
	} else 	if(mlStringCompare(prhs[0], "INSGpsCorrection")) {

		if(nrhs != 3) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], pos_data, 3) ||
				!mlGetFloatArray(prhs[2], vel_data ,3)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		GpsCorrection(pos_data, vel_data);
	} else 	if(mlStringCompare(prhs[0], "INSVelBaroCorrection")) {

		if(nrhs != 3) {
			mexErrMsgTxt("Incorrect number of inputs for correction\n");
			return;
		}

		if(!mlGetFloatArray(prhs[1], vel_data, 3) ||
				!mlGetFloatArray(prhs[2], &baro_data, 1)) {
			mexErrMsgTxt("Error with the input parameters\n");
			return;
		}

		VelBaroCorrection(vel_data, baro_data);
	} else if (mlStringCompare(prhs[0], "INSSetPosVelVar")) {
		float pos_var;
        float vel_var;
		if((nrhs != 3) || !mlGetFloatArray(prhs[1], &pos_var, 1) ||
                !mlGetFloatArray(prhs[2], &vel_var, 1)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		INSSetPosVelVar(pos_var, vel_var);
	} else if (mlStringCompare(prhs[0], "INSSetGyroBias")) {
		float gyro_bias[3];
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_bias, 3)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		INSSetGyroBias(gyro_bias);
	} else if (mlStringCompare(prhs[0], "INSSetAccelVar")) {
		float accel_var[3];
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], accel_var, 3)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		INSSetAccelVar(accel_var);
	} else if (mlStringCompare(prhs[0], "INSSetGyroVar")) {
		float gyro_var[3];
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], gyro_var, 3)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		INSSetGyroVar(gyro_var);
	} else if (mlStringCompare(prhs[0], "INSSetMagNorth")) {
		float mag_north[3];
		float Bmag;
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_north, 3)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		Bmag = sqrt(mag_north[0] * mag_north[0] + mag_north[1] * mag_north[1] +
				mag_north[2] * mag_north[2]);
		mag_north[0] = mag_north[0] / Bmag;
		mag_north[1] = mag_north[1] / Bmag;
		mag_north[2] = mag_north[2] / Bmag;

		INSSetMagNorth(mag_north);
	} else if (mlStringCompare(prhs[0], "INSSetMagVar")) {
		float mag_var[3];
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], mag_var, 3)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		INSSetMagVar(mag_var);
	} else if (mlStringCompare(prhs[0], "INSSetState")) {
        int i;
		float new_state[NUMX];
		if((nrhs != 2) || !mlGetFloatArray(prhs[1], new_state, NUMX)) {
			mexErrMsgTxt("Error with input parameters\n");
			return;
		}
		for(i = 0; i < NUMX; i++)
			X[i] = new_state[i];
	} else {
		mexErrMsgTxt("Unknown function");
	}

	if(nlhs > 0) {
		// return current state vector
		double * data_out;
		int i;

		plhs[0] = mxCreateDoubleMatrix(1,13,0);
		data_out = mxGetData(plhs[0]);
		for(i = 0; i < NUMX; i++)
			data_out[i] = X[i];
	}

	if(nlhs > 1) {
		//return covariance estimate
		double * data_copy = mxCalloc(NUMX*NUMX, sizeof(double));
		int i, j, k;

		plhs[1] = mxCreateDoubleMatrix(13,13,0);
		for(i = 0; i < NUMX; i++)
			for(j = 0; j < NUMX; j++)
			{
				data_copy[j + i * NUMX] = P[j][i];
			}

		mxSetData(plhs[1], data_copy);
	}

	if(nlhs > 2) {
		//return covariance estimate
		double * data_copy = mxCalloc(NUMX*NUMV, sizeof(double));
		int i, j, k;

		plhs[2] = mxCreateDoubleMatrix(NUMX,NUMV,0);
		for(i = 0; i < NUMX; i++)
			for(j = 0; j < NUMV; j++)
			{
				data_copy[j + i * NUMX] = K[i][j];
			}

		mxSetData(plhs[2], data_copy);
	}
	return;
}
Пример #2
0
int main()
{
	float gyro[3], accel[3], mag[3];
	float vel[3] = { 0, 0, 0 };
	/* Normaly we get/set UAVObjects but this one only needs to be set.
	We will never expect to get this from another module*/
	AttitudeActualData attitude_actual;
	AHRSSettingsData ahrs_settings;

	/* Brings up System using CMSIS functions, enables the LEDs. */
	PIOS_SYS_Init();

	/* Delay system */
	PIOS_DELAY_Init();

	/* Communication system */
	PIOS_COM_Init();

	/* ADC system */
	AHRS_ADC_Config( adc_oversampling );

	/* Setup the Accelerometer FS (Full-Scale) GPIO */
	PIOS_GPIO_Enable( 0 );
	SET_ACCEL_2G;
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
	/* Magnetic sensor system */
	PIOS_I2C_Init();
	PIOS_HMC5843_Init();

	// Get 3 ID bytes
	strcpy(( char * )mag_data.id, "ZZZ" );
	PIOS_HMC5843_ReadID( mag_data.id );
#endif

	/* SPI link to master */
//	PIOS_SPI_Init();
	AhrsInitComms();
	AHRSCalibrationConnectCallback( calibration_callback );
	GPSPositionConnectCallback( gps_callback );

	ahrs_state = AHRS_IDLE;

	while( !AhrsLinkReady() ) {
		AhrsPoll();
		while( ahrs_state != AHRS_DATA_READY ) ;
		ahrs_state = AHRS_PROCESSING;
		downsample_data();
		ahrs_state = AHRS_IDLE;
		if(( total_conversion_blocks % 50 ) == 0 )
			PIOS_LED_Toggle( LED1 );
	}


	AHRSSettingsGet(&ahrs_settings);


	/* Use simple averaging filter for now */
	for( int i = 0; i < adc_oversampling; i++ )
		fir_coeffs[i] = 1;
	fir_coeffs[adc_oversampling] = adc_oversampling;

	if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_INSGPS) {
		// compute a data point and initialize INS
		downsample_data();
		converge_insgps();
	}


#ifdef DUMP_RAW
	int previous_conversion;
	while( 1 ) {
		AhrsPoll();
		int result;
		uint8_t framing[16] = {
			0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
			15
		};
		while( ahrs_state != AHRS_DATA_READY ) ;
		ahrs_state = AHRS_PROCESSING;

		if( total_conversion_blocks != previous_conversion + 1 )
			PIOS_LED_On( LED1 );	// not keeping up
		else
			PIOS_LED_Off( LED1 );
		previous_conversion = total_conversion_blocks;

		downsample_data();
		ahrs_state = AHRS_IDLE;;

		// Dump raw buffer
		result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 );	// framing header
		result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) );	// dump block number
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & valid_data_buffer[0],
								 ADC_OVERSAMPLE *
								 ADC_CONTINUOUS_CHANNELS *
								 sizeof( valid_data_buffer[0] ) );
		if( result == 0 )
			PIOS_LED_Off( LED1 );
		else {
			PIOS_LED_On( LED1 );
		}
	}
#endif

	timer_start();

	/******************* Main EKF loop ****************************/
	while( 1 ) {
		AhrsPoll();
		AHRSCalibrationData calibration;
		AHRSCalibrationGet( &calibration );
		BaroAltitudeData baro_altitude;
		BaroAltitudeGet( &baro_altitude );
		GPSPositionData gps_position;
		GPSPositionGet( &gps_position );
		AHRSSettingsGet(&ahrs_settings);

		// Alive signal
		if(( total_conversion_blocks % 100 ) == 0 )
			PIOS_LED_Toggle( LED1 );

#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
		// Get magnetic readings
		if( PIOS_HMC5843_NewDataAvailable() ) {
			PIOS_HMC5843_ReadMag( mag_data.raw.axis );
			mag_data.updated = 1;
		}
		attitude_raw.magnetometers[0] = mag_data.raw.axis[0];
		attitude_raw.magnetometers[2] = mag_data.raw.axis[1];
		attitude_raw.magnetometers[2] = mag_data.raw.axis[2];

#endif
		// Delay for valid data

		counter_val = timer_count();
		running_counts = counter_val - last_counter_idle_end;
		last_counter_idle_start = counter_val;

		while( ahrs_state != AHRS_DATA_READY ) ;

		counter_val = timer_count();
		idle_counts = counter_val - last_counter_idle_start;
		last_counter_idle_end = counter_val;

		ahrs_state = AHRS_PROCESSING;

		downsample_data();

		/***************** SEND BACK SOME RAW DATA ************************/
		// Hacky - grab one sample from buffer to populate this.  Need to send back
		// all raw data if it's happening
		accel_data.raw.x = valid_data_buffer[0];
		accel_data.raw.y = valid_data_buffer[2];
		accel_data.raw.z = valid_data_buffer[4];

		gyro_data.raw.x = valid_data_buffer[1];
		gyro_data.raw.y = valid_data_buffer[3];
		gyro_data.raw.z = valid_data_buffer[5];

		gyro_data.temp.xy = valid_data_buffer[6];
		gyro_data.temp.z = valid_data_buffer[7];

		if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_INSGPS) {
			/******************** INS ALGORITHM **************************/

			// format data for INS algo
			gyro[0] = gyro_data.filtered.x;
			gyro[1] = gyro_data.filtered.y;
			gyro[2] = gyro_data.filtered.z;
			accel[0] = accel_data.filtered.x,
					   accel[1] = accel_data.filtered.y,
								  accel[2] = accel_data.filtered.z,
											 // Note: The magnetometer driver returns registers X,Y,Z from the chip which are
											 // (left, backward, up).  Remapping to (forward, right, down).
											 mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] );
			mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] );
			mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] );

			INSStatePrediction( gyro, accel,
								1 / ( float )EKF_RATE );
			INSCovariancePrediction( 1 / ( float )EKF_RATE );

			if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) {
				// Compute velocity from Heading and groundspeed
				vel[0] =
					gps_position.Groundspeed *
					cos( gps_position.Heading * M_PI / 180 );
				vel[1] =
					gps_position.Groundspeed *
					sin( gps_position.Heading * M_PI / 180 );

				// Completely unprincipled way to make the position variance
				// increase as data quality decreases but keep it bounded
				// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
				INSSetPosVelVar( 0.004 );

				HomeLocationData home;
				HomeLocationGet( &home );
				float ned[3];
				double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )};
				// convert from cm back to meters
				double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )};
				LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned );

				if( gps_updated ) { //FIXME: Is this correct?
					//TOOD: add check for altitude updates
					FullCorrection( mag, ned,
									vel,
									baro_altitude.Altitude );
					gps_updated = false;
				} else {
					GpsBaroCorrection( ned,
									   vel,
									   baro_altitude.Altitude );
				}

				gps_updated = false;
				mag_data.updated = 0;
			} else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D
					   && mag_data.updated == 1 ) {
				MagCorrection( mag );	// only trust mags if outdoors
				mag_data.updated = 0;
			} else {
				// Indoors, update with zero position and velocity and high covariance
				INSSetPosVelVar( 0.1 );
				vel[0] = 0;
				vel[1] = 0;
				vel[2] = 0;

				VelBaroCorrection( vel,
								   baro_altitude.Altitude );
//                MagVelBaroCorrection(mag,vel,altitude_data.altitude);  // only trust mags if outdoors
			}

			attitude_actual.q1 = Nav.q[0];
			attitude_actual.q2 = Nav.q[1];
			attitude_actual.q3 = Nav.q[2];
			attitude_actual.q4 = Nav.q[3];
		} else if( ahrs_settings.Algorithm ==  AHRSSETTINGS_ALGORITHM_SIMPLE ) {
			float q[4];
			float rpy[3];
			/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
			/* Very simple computation of the heading and attitude from accel. */
			rpy[2] =
				atan2(( mag_data.raw.axis[0] ),
					  ( -1 * mag_data.raw.axis[1] ) ) * 180 /
				M_PI;
			rpy[1] =
				atan2( accel_data.filtered.x,
					   accel_data.filtered.z ) * 180 / M_PI;
			rpy[0] =
				atan2( accel_data.filtered.y,
					   accel_data.filtered.z ) * 180 / M_PI;

			RPY2Quaternion( rpy, q );
			attitude_actual.q1 = q[0];
			attitude_actual.q2 = q[1];
			attitude_actual.q3 = q[2];
			attitude_actual.q4 = q[3];
		}

		ahrs_state = AHRS_IDLE;

#ifdef DUMP_FRIENDLY
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n",
				total_conversion_blocks );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n",
				( int16_t )( accel_data.filtered.x * 1000 ),
				( int16_t )( accel_data.filtered.y * 1000 ),
				( int16_t )( accel_data.filtered.z * 1000 ) );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n",
				( int16_t )( gyro_data.filtered.x * 1000 ),
				( int16_t )( gyro_data.filtered.y * 1000 ),
				( int16_t )( gyro_data.filtered.z * 1000 ) );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n",
				mag_data.raw.axis[0],
				mag_data.raw.axis[1],
				mag_data.raw.axis[2] );
		PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX,
				"q: %d %d %d %d\r\n",
				( int16_t )( Nav.q[0] * 1000 ),
				( int16_t )( Nav.q[1] * 1000 ),
				( int16_t )( Nav.q[2] * 1000 ),
				( int16_t )( Nav.q[3] * 1000 ) );
#endif
#ifdef DUMP_EKF
		uint8_t framing[16] = {
			15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1,
			0
		};
		extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
		extern float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
		extern float Q[NUMW], R[NUMV];	// input noise and measurement noise variances
		extern float K[NUMX][NUMV];	// feedback gain matrix

		// Dump raw buffer
		int8_t result;
		result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 );	// framing header
		result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) );	// dump block number
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & mag_data,
								 sizeof( mag_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & gps_data,
								 sizeof( gps_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & accel_data,
								 sizeof( accel_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX,
								 ( uint8_t * ) & gyro_data,
								 sizeof( gyro_data ) );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q,
								 sizeof( float ) * NUMX * NUMX );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K,
								 sizeof( float ) * NUMX * NUMV );
		result +=
			PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X,
								 sizeof( float ) * NUMX * NUMX );

		if( result == 0 )
			PIOS_LED_Off( LED1 );
		else {
			PIOS_LED_On( LED1 );
		}
#endif
		AttitudeActualSet( &attitude_actual );

		/*FIXME: This is dangerous. There is no locking for UAVObjects
		so it could stomp all over the airspeed/climb rate etc.
		This used to be done in the OP module which was bad.
		Having ~4ms latency for the round trip makes it worse here.
		*/
		PositionActualData pos;
		PositionActualGet( &pos );
		for( int ct = 0; ct < 3; ct++ ) {
			pos.NED[ct] = Nav.Pos[ct];
			pos.Vel[ct] = Nav.Vel[ct];
		}
		PositionActualSet( &pos );

		static bool was_calibration = false;
		AhrsStatusData status;
		AhrsStatusGet( &status );
		if( was_calibration != status.CalibrationSet ) {
			was_calibration = status.CalibrationSet;
			if( status.CalibrationSet ) {
				calibrate_sensors();
				AhrsStatusGet( &status );
				status.CalibrationSet = true;
			}
		}
		status.CPULoad = (( float )running_counts /
						  ( float )( idle_counts + running_counts ) ) * 100;

		status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 );
		status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 );
		status.DroppedUpdates = ekf_too_slow;
		AhrsStatusSet( &status );

	}

	return 0;
}