uint32_t imu_get_euler_angle(float *adata, float *gdata, float *mdata, attitude_t *angle)
{
MadgwickAHRSupdate( (float)gdata[0]*PI/180,
(float)gdata[1]*PI/180,
(float)gdata[2]*PI/180,
(float)adata[0],
(float)adata[1],
(float)adata[2],
(float)mdata[0],
(float)mdata[1],
(float)mdata[2],
angle);
return 0;
}
/*
* Madgwick stream thread
*/
static msg_t stream_madgwick_thread(void *arg) {
attitude_t attitude_data;
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
while (TRUE) {
float mx = ((float)mag_data.x - (-440.0)) / (510 - (-440)) * 2 - 1.0;
float my = ((float)mag_data.y - (-740.0)) / (380 - (-740)) * 2 - 1.0;
float mz = ((float)mag_data.z - (-500.0)) / (500 - (-500)) * 2 - 1.0;
MadgwickAHRSupdate((-gyro_data.x / 57.143) * 3.141592 / 180.0,
(gyro_data.y / 57.143) * 3.141592 / 180.0,
-(gyro_data.z / 57.143) * 3.141592 / 180.0,
-acc_data.x / 1000.0, acc_data.y / 1000.0,
acc_data.z / 1000.0, mx, -my, -mz);
getMadAttitude(&attitude_data);
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f\r\n", (int)time, attitude_data.roll,
attitude_data.pitch, attitude_data.yaw);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
int main()
{
// double gpsTime;
// unsigned short gpsWeek, status;
// VnVector3 ypr, latitudeLognitudeAltitude, nedVelocity;
// float attitudeUncertainty, positionUncertainty, velocityUncertainty;
VnVector3 magnetic, acceleration, angularRate;
float temperature, pressure;
float mx, my, mz, ax, ay, az, gx, gy, gz;
Vn200 vn200;
// Vn100 vn100;
int i = 0;
vn200_connect(&vn200, COM_PORT, BAUD_RATE);
while(1) {
vn200_getCalibratedSensorMeasurements(
&vn200,
&magnetic,
&acceleration,
&angularRate,
&temperature,
&pressure);
mx = (float) magnetic.c0;
my = (float) magnetic.c1;
mz = (float) magnetic.c2;
ax = (float) acceleration.c0;
ay = (float) acceleration.c1;
az = (float) acceleration.c2;
gx = (float) angularRate.c0;
gy = (float) angularRate.c1;
gz = (float) angularRate.c2;
MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz);
// printf("IMU Solution: \n"
// "i: %d\n"
// "magnetic.x: %+#7.2f\n"
// "magnetic.y: %+#7.2f\n"
// "magnetic.z: %+#7.2f\n"
// "acceleration.x: %+#7.2f\n"
// "acceleration.y: %+#7.2f\n"
// "acceleration.z: %+#7.2f\n"
// "angularRate.x: %+#7.2f\n"
// "angularRate.y: %+#7.2f\n"
// "angularRate.z: %+#7.2f\n",
// i,
// magnetic.c0,
// magnetic.c1,
// magnetic.c2,
// acceleration.c0,
// acceleration.c1,
// acceleration.c2,
// angularRate.c0,
// angularRate.c1,
// angularRate.c2);
// vn200_getInsSolution(
// &vn200,
// &gpsTime,
// &gpsWeek,
// &status,
// &ypr,
// &latitudeLognitudeAltitude,
// &nedVelocity,
// &attitudeUncertainty,
// &positionUncertainty,
// &velocityUncertainty);
// printf("INS Solution:\n"
// " i: %d\n"
// " GPS Time: %f\n"
// " GPS Week: %u\n"
// " INS Status: %.4X\n"
// " YPR.Yaw: %+#7.2f\n"
// " YPR.Pitch: %+#7.2f\n"
// " YPR.Roll: %+#7.2f\n"
// " LLA.Lattitude: %+#7.2f\n"
// " LLA.Longitude: %+#7.2f\n"
// " LLA.Altitude: %+#7.2f\n"
// " Velocity.North: %+#7.2f\n"
// " Velocity.East: %+#7.2f\n"
// " Velocity.Down: %+#7.2f\n"
// " Attitude Uncertainty: %+#7.2f\n"
// " Position Uncertainty: %+#7.2f\n"
// " Velocity Uncertainty: %+#7.2f\n",
// i,
// gpsTime,
// gpsWeek,
// status,
// ypr.c0,
// ypr.c1,
// ypr.c2,
// latitudeLognitudeAltitude.c0,
// latitudeLognitudeAltitude.c1,
// latitudeLognitudeAltitude.c2,
// nedVelocity.c0,
// nedVelocity.c1,
// nedVelocity.c2,
// attitudeUncertainty,
// positionUncertainty,
// velocityUncertainty);
printf("\n\n");
// sleep(0.05);
i++;
}
vn200_disconnect(&vn200);
return 0;
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
RCC_GetClocksFreq(&clocks);
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
// Wait until device configured
//while(bDeviceState != CONFIGURED);
testInit();
LED0_ON;
systemReady = true;
//nrf_tx_mode_no_aa(addr,5,40);
nrf_rx_mode_dual(addr,5,40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while (1)
{
uint8_t buf[64];
static uint8_t last_tx_done = 0;
if(ring_buf_pop(nrf_rx_buffer,buf,32)){
// get data from the adapter
switch(buf[0]){
case SET_ATT:
break;
case SET_MOTOR:
break;
case SET_MODE:
report_mode = buf[1];
break;
}
last_tx_done = 1;
}
if(tx_done){
tx_done = 0;
// report ACK success
last_tx_done = 1;
}
if(ring_buf_pop(nrf_tx_buffer,buf,32)){
if(last_tx_done){
last_tx_done = 0;
nrf_ack_packet(0, buf, 32);
}
}
if (frame_50Hz)
{
int16_t motor_val[4];
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
//memcpy(buf, accelSummedSamples100Hz, 12);
//memcpy(buf+12, gyroSummedSamples100Hz, 12);
//memcpy(buf+24, magSumed, 6);
if(report_mode == DT_ATT){
buf[0] = DT_ATT;
memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
memcpy(buf + 13, &executionTime200Hz, 4);
motor_val[0] = motor[0];
motor_val[1] = motor[1];
motor_val[2] = motor[2];
motor_val[3] = motor[3];
memcpy(buf + 17, motor_val, 8);
usb_send_data(buf , 64);
executionTime50Hz = micros() - currentTime;
}else if(report_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
memcpy(buf + 1, gyroSummedSamples100Hz, 12);
memcpy(buf + 13, accelSummedSamples100Hz, 12);
memcpy(buf + 25, magSumed, 6);
}
//nrf_tx_packet(buf,16);
//if(nrf_rx_packet(buf,16) == NRF_RX_OK)
//{
// LED0_TOGGLE;
//}
ring_buf_push(nrf_tx_buffer, buf, 32);
}
if(frame_10Hz)
{
frame_10Hz = false;
magSumed[XAXIS] = magSum[XAXIS];
magSumed[YAXIS] = magSum[YAXIS];
magSumed[ZAXIS] = magSum[ZAXIS];
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
}
if (frame_100Hz)
{
frame_100Hz = false;
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
}
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
}
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
LED0_TOGGLE;
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
/**
* @brief Main IMU function that should be called all the time in the main while loop. It updates the values
of the sensors, calculates the real values from the raw reading and does the Madgwick algorithm to
calculate the Euler angles from the thes IMU readings when the data is ready. The dataReady flags are
set in the interrupt.cpp file in the ISR(TWI_vect) when the data is ready. The time since the last
update is also measured independently for each sensor to get the effective update rate of each
sensor. This is done by dividing sum variable (which is the total time since the last sensor rate
update) by the sumCount variable (which is the number of readings taken since
the last sensor rate update). These variables are global and can be used anywhere to measure this
update rate. These values must be put to 0 after the rate is taken.
*/
void IMU::updateImuAndMadgwick()
{
if(ak8963DataReady)
{
ak8963DataReady = false;
ak8963.setRawMagneticField(currentRawMag);
ak8963.calculateMag();
//calculate the time it takes before the next call of this function
Now = TCNT0;
//the 8 is counter 0 prescaler
deltaTimeAk8963 = ((Now - lastUpdateAk8963 + OCR0A*timer0OverflowCountAk8963)*8/(float)F_CPU);
timer0OverflowCountAk8963 = 0;
lastUpdateAk8963 = Now;
sumAk8963 += deltaTimeAk8963; // sum for averaging filter update rate
sumCountAk8963++;
// Page 38 of PS-MPU-9255 gives the axes orientations
// the x axis on the MPU9255 represents the forward direction. Since on our board the front is actually the y
// direction, a 90 degree rotation clockwise has to be made. For this x = y and y = -x for all 3 sensors.
// on top of this, to make sure the convention of positive pitch yaw roll is respected, the final x is multiplied by -1
// for all sensors (x,y,z) becomes (-y,-x,z)
// for the magnetometer, an extra modification is required
// Sensors x (y)-axis of the accelerometer/gyro is aligned with the y (x)-axis of the magnetometer;
// the magnetometer z-axis (+ down) is misaligned with z-axis (+ up) of accelerometer and gyro!
// so we interchange x and y and multiply the Z axis by -1.
// the total change is (x,y,z) becomes (-x,-y,-z))
xMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldX();
yMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldY();
zMagneticFieldMadgwick = -(float)ak8963.getMagneticFieldZ();
MadgwickAHRSupdate(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick,
xRotationMadgwick, yRotationMadgwick, zRotationMadgwick,
xMagneticFieldMadgwick, yMagneticFieldMadgwick, zMagneticFieldMadgwick);
calcEulerAngles();
}
else if(bmp180DataReady)
{
bmp180DataReady = false;
bmp180.setRawTemperature(currentRawTemp);
bmp180.setRawPressure(currentRawPress);
bmp180.calculateTrueValues();
//calculate the time it takes before the next call of this function
Now = TCNT0;
//the 8 is counter 0 prescaler
deltaTimeBmp180 = ((Now - lastUpdateBmp180 + OCR0A*timer0OverflowCountBmp180)*8/(float)F_CPU);
timer0OverflowCountBmp180 = 0;
lastUpdateBmp180 = Now;
sumBmp180 += deltaTimeBmp180; // sum for averaging filter update rate
sumCountBmp180++;
}
else if(mpu9255DataReady)
{
mpu9255DataReady = false;
//These functions updates the values of the class variables with the FSM buffer variables
mpu9255.setRawAcceleration(currentRawAcc);
mpu9255.setRawRotation(currentRawGyr);
mpu9255.calculateAccRotTemp();
//calculate the time it takes before the next call of this function
Now = TCNT0;
//the 8 is counter 0 prescaler
deltaTimeMpu9255 = ((Now - lastUpdateMpu9255 + OCR0A*timer0OverflowCountMpu9255)*8/(float)F_CPU);
timer0OverflowCountMpu9255 = 0;
lastUpdateMpu9255 = Now;
sumMpu9255 += deltaTimeMpu9255; // sum for averaging filter update rate
sumCountMpu9255++;
xAccelerationMadgwick = -(float)mpu9255.getAccelerationY()/10000;
yAccelerationMadgwick = -(float)mpu9255.getAccelerationX()/10000;
zAccelerationMadgwick = (float)mpu9255.getAccelerationZ()/10000;
xRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800;
yRotationMadgwick = -(float)mpu9255.getRotationY()*M_PI_OVER_1800;
zRotationMadgwick = (float)mpu9255.getRotationZ()*M_PI_OVER_1800;
MadgwickAHRSupdateIMU(xAccelerationMadgwick, yAccelerationMadgwick, zAccelerationMadgwick,
xRotationMadgwick, yRotationMadgwick, zRotationMadgwick);
calcEulerAngles();
}
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
void MainWindow::processSensorsData(TUdpDataSENSORS* data)
{
int32_t time = data->ticks;
/*m_pdaData.frustum[0]*=10;
m_pdaData.frustum[1]*=10;
m_pdaData.frustum[2]*=10;
m_pdaData.frustum[3]*=10;*/
m_pdaData.sphereEnabled = data->sphereEnabled;
m_pdaData.ax = -data->ax;
m_pdaData.ay = -data->ay;
m_pdaData.az = -data->az;
m_pdaData.gx = data->gx;
m_pdaData.gy = data->gy;
m_pdaData.gz = data->gz;
m_pdaData.mx = -data->mx;
m_pdaData.my = -data->my;
m_pdaData.mz = -data->mz;
m_pdaData.worldQuat = QQuaternion(data->q0, data->q1, data->q2, data->q3);
if (m_pdaData.mx < minX) minX = m_pdaData.mx;
if (m_pdaData.my < minY) minY = m_pdaData.my;
if (m_pdaData.mz < minZ) minZ = m_pdaData.mz;
if (m_pdaData.mx > maxX) maxX = m_pdaData.mx;
if (m_pdaData.my > maxY) maxY = m_pdaData.my;
if (m_pdaData.mz > maxZ) maxZ = m_pdaData.mz;
//-66.75 -46.75 -40.25
// 40.75 40.5 40.5
static int64_t last = 0;
if (last == 0)
last = time;
int64_t diff = time - last;
if (diff > 1000)
{
//qDebug() << "RES";
m_accelQuat = QQuaternion(1, 0, 0, 0);
m_gyroQuat = QQuaternion(1, 0, 0, 0);
m_gyroAccelQuat = QQuaternion(1, 0, 0, 0);
m_accelMagnetQuat = QQuaternion(1, 0, 0, 0);
m_fullQuat = QQuaternion(1, 0, 0, 0);
last = 0;
diff = 100;
return;
}
last = time;
//qDebug() << "DIFF" << time << diff;
MadgwickAHRSupdateIMU(m_accelQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f * 5);
MadgwickAHRSupdateIMU(m_gyroQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, 0, 0, 0, (float)diff / 1000.0f);
MadgwickAHRSupdateIMU(m_gyroAccelQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, (float)diff / 1000.0f);
MadgwickAHRSupdate(m_accelMagnetQuat, 0, 0, 0, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f);
MadgwickAHRSupdate(m_fullQuat, m_pdaData.gx, m_pdaData.gy, m_pdaData.gz, m_pdaData.ax, m_pdaData.ay, m_pdaData.az, m_pdaData.mx, m_pdaData.my, m_pdaData.mz, (float)diff / 1000.0f);
ui->visAccel->rotationQuat = m_accelQuat;
ui->visGyro->rotationQuat = m_gyroQuat;
ui->visGyroAccel->rotationQuat = m_gyroAccelQuat;
ui->visAccelMagnet->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90);
ui->visAccelMagnet->rotationQuat *= m_accelMagnetQuat;
ui->visFull->rotationQuat = QQuaternion::fromAxisAndAngle(0, 0, 1, -90);
ui->visFull->rotationQuat *= m_fullQuat;
ui->visWorld->rotationQuat = m_pdaData.worldQuat.conjugate();
ui->visWorld2->rotationQuat = m_pdaData.worldQuat.conjugate();
ui->visScreen->rotationQuat = m_pdaData.worldQuat.conjugate();
//static float q = 0;
//ui->visWorld->rotationQuat *= QQuaternion::fromAxisAndAngle(0, 1, 1, q += 1);
float qq0 = m_fullQuat.scalar();
float qq1 = m_fullQuat.x();
float qq2 = m_fullQuat.y();
float qq3 = m_fullQuat.z();
float q2_2 = qq2*qq2;
m_pdaData.pitch = atanf (2.0f*(qq0*qq1 + qq2*qq3) / (1.0f - 2.0f*(qq1*qq1 + q2_2)));
m_pdaData.roll = asinf (2.0f*(qq0*qq2 - qq3*qq1));
m_pdaData.yaw = atan2f (2.0f*(qq0*qq3 + qq1*qq2), (1.0f - 2.0f*(q2_2 + qq3*qq3)));
ui->rawReads->updateInfo(m_pdaData);
}
// Main loop
void razor_loop(void)
{
razor_loop_input();
// Time to read the sensors again?
if ((timer_systime() - timestamp) >= OUTPUT__DATA_INTERVAL)
{
timestamp_old = timestamp;
timestamp = timer_systime();
if (timestamp > timestamp_old)
G_Dt = (double)MS2S(timestamp - timestamp_old); // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0.0;
// Update sensor readings
read_sensors();
switch (output_mode)
{
case OUTPUT__MODE_CALIBRATE_SENSORS:
// We're in calibration mode
{
check_reset_calibration_session(); // Check if this session needs a reset
if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
break;
}
case OUTPUT__MODE_YPR_RAZOR:
{
// Apply sensor calibration
compensate_sensor_errors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_MADGWICK:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// Apply Madgwick algorithm
MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], magnetom[0], magnetom[1], magnetom[2]);
//MadgwickAHRSupdate(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, magnetom[0], magnetom[1], magnetom[2]);
MadgwickYawPitchRoll(&yaw, &pitch, &roll);
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_YPR_FREEIMU:
{
compensate_sensor_errors();
gyro[0] = GYRO_SCALED_RAD(gyro[0]);
gyro[1] = GYRO_SCALED_RAD(gyro[1]);
gyro[2] = GYRO_SCALED_RAD(gyro[2]);
// acc and mag already scaled in compensate_sensor_errors()
// get sensorManager and initialise sensor listeners
//mSensorManager = (SensorManager) this.getSystemService(SENSOR_SERVICE);
//initListeners();
// wait for one second until gyroscope and magnetometer/accelerometer
// data is initialised then scedule the complementary filter task
//fuseTimer.scheduleAtFixedRate(new calculateFusedOrientationTask(), 1000, TIME_CONSTANT);
//public void onSensorChanged(SensorEvent event) {
//switch(event.sensor.getType()) {
//case Sensor.TYPE_ACCELEROMETER:
// copy new accelerometer data into accel array and calculate orientation
//System.arraycopy(event.values, 0, accel, 0, 3);
calculateAccMagOrientation();
//case Sensor.TYPE_GYROSCOPE:
// process gyro data
//gyroFunction(event);
gyroFunction();
//case Sensor.TYPE_MAGNETIC_FIELD:
// copy new magnetometer data into magnet array
//System.arraycopy(event.values, 0, magnet, 0, 3);
calculateFusedOrientation();
yaw = -fusedOrientation[0];
roll = -fusedOrientation[1];
pitch = fusedOrientation[2];
//if (output_stream_on || output_single_on) output_angles_freedom();
if (output_stream_on || output_single_on) output_angles();
break;
}
case OUTPUT__MODE_SENSORS_RAW:
case OUTPUT__MODE_SENSORS_CALIB:
case OUTPUT__MODE_SENSORS_BOTH:
{
if (output_stream_on || output_single_on) output_sensors();
break;
}
}
output_single_on = false;
#if DEBUG__PRINT_LOOP_TIME == true
//printf("loop time (ms) = %lu\r\n", timer_systime() - timestamp);
// Not really useful since RTC measures only 4 ms.
#endif
}
#if DEBUG__PRINT_LOOP_TIME == true
else
{
printf("waiting...\r\n");
}
#endif
return;
}
void
psmove_orientation_update(PSMoveOrientation *orientation)
{
psmove_return_if_fail(orientation != NULL);
int frame;
long now = psmove_util_get_ticks();
if (now - orientation->sample_freq_measure_start >= 1000) {
float measured = ((float)orientation->sample_freq_measure_count) /
((float)(now-orientation->sample_freq_measure_start))*1000.;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation->sample_freq = measured;
orientation->sample_freq_measure_start = now;
orientation->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation->sample_freq_measure_count += 2;
psmove_get_magnetometer_vector(orientation->move,
&orientation->output[6],
&orientation->output[7],
&orientation->output[8]);
float q0 = orientation->quaternion[0];
float q1 = orientation->quaternion[1];
float q2 = orientation->quaternion[2];
float q3 = orientation->quaternion[3];
for (frame=0; frame<2; frame++) {
psmove_get_accelerometer_frame(orientation->move, frame,
&orientation->output[0],
&orientation->output[1],
&orientation->output[2]);
psmove_get_gyroscope_frame(orientation->move, frame,
&orientation->output[3],
&orientation->output[4],
&orientation->output[5]);
#if defined(PSMOVE_WITH_MADGWICK_AHRS)
MadgwickAHRSupdate(orientation->quaternion,
orientation->sample_freq,
0.5f,
/* Gyroscope */
orientation->output[3],
orientation->output[5],
-orientation->output[4],
/* Accelerometer */
orientation->output[0],
orientation->output[2],
-orientation->output[1],
/* Magnetometer */
orientation->output[6],
orientation->output[8],
orientation->output[7]
);
#else
psmove_DEBUG("Built without Madgwick AHRS - no orientation tracking");
return;
#endif
if (isnan(orientation->quaternion[0]) ||
isnan(orientation->quaternion[1]) ||
isnan(orientation->quaternion[2]) ||
isnan(orientation->quaternion[3])) {
psmove_DEBUG("Orientation is NaN!");
orientation->quaternion[0] = q0;
orientation->quaternion[1] = q1;
orientation->quaternion[2] = q2;
orientation->quaternion[3] = q3;
}
}
}
int main (int argc, char* argv[])
{
float gx = 0.0;
float gy = 0.0;
float gz = 0.0;
float ax = 1.0;
float ay = 1.0;
float az = 1.0;
float mx = 0.0;
float my = 0.0;
float mz = 0.0;
int i;
printf("Size of float is %d\n", sizeof(float));
printf ("%f, %f, %f, %f\n", q0, q1, q2, q3);
for (i = 0; i < 100; i += 1) {
MadgwickAHRSupdate(gx, gy, gz, ax, ay, az, mx, my, mz);
printf ("%f, %f, %f, %f\n", q0, q1, q2, q3);
}
return(0);
}
void loop(){
static int i=0;
static float goffsetx=0,goffsety=0,goffsetz=0;
static float coef = 3.1415926599f/180;
if(sensor_data_ready_FLAG){
sensor_data_ready_FLAG=FALSE;
if(i<1000){
//calc gyro offset with 1000 samples
goffsetx+=local_data.gyro[0];
goffsety+=local_data.gyro[1];
goffsetz+=local_data.gyro[2];
i++;
if(i==1000){
BSP_LED_On(LED0);
goffsetx/=1000;
goffsety/=1000;
goffsetz/=1000;
}
}else{
MadgwickAHRSupdate(q, 1.0f/SAMPLINGRATE,(local_data.gyro[0]-goffsetx)*coef,(local_data.gyro[1]-goffsety)*coef,(local_data.gyro[2]-goffsetz)*coef,local_data.acc[0],local_data.acc[1],local_data.acc[2],local_data.mag[1]*-1,local_data.mag[0]*-1,local_data.mag[2]);
}
sensor_print(NULL);
//static char tmpbuf[30];
/*memset(tmpbuf,0,30);
sprintf(tmpbuf,"gyro %.2f %.2f %.2f ",data->gyro[0],data->gyro[1],data->gyro[2]);
UART_Transmit((uint8_t *)tmpbuf,30);
memset(tmpbuf,0,30);
sprintf(tmpbuf,"acc %.2f %.2f %.2f\n",data->acc[0],data->acc[1],data->acc[2]);
UART_Transmit((uint8_t *)tmpbuf,30);
memset(tmpbuf,0,30);
sprintf(tmpbuf,"mag %.2f %.2f %.2f\n",data->mag[0],data->mag[1],data->mag[2]);
UART_Transmit((uint8_t *)tmpbuf,30);*/
}
}
