void sample_MiniMU9() //Main Loop
{
if((millis()-timer)>=100) //20 // Main loop runs at 50Hz
{
counter++;
timer_old = timer;
timer=millis();
if (timer>timer_old)
G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else
G_Dt = 0;
// *** DCM algorithm
// Data adquisition
Read_Gyro(); // This read gyro data
Read_Accel(); // Read I2C accelerometer
if (counter > 5) // Read compass data at 10Hz... (5 loop runs)
{
counter=0;
Read_Compass(); // Read I2C magnetometer
Compass_Heading(); // Calculate magnetic heading
}
// Calculations...
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// ***
printdata();
}
}
void ahrs_dcm_update_accel(struct Int32Vect3 *accel)
{
ACCELS_FLOAT_OF_BFP(accel_float, *accel);
// DCM filter uses g-force as positive
// accelerometer measures [0 0 -g] in a static case
accel_float.x = -accel_float.x;
accel_float.y = -accel_float.y;
accel_float.z = -accel_float.z;
ahrs_dcm.gps_age ++;
if (ahrs_dcm.gps_age < 50) { //Remove centrifugal acceleration and longitudinal acceleration
#if USE_AHRS_GPS_ACCELERATIONS
PRINT_CONFIG_MSG("AHRS_FLOAT_DCM uses GPS acceleration.")
accel_float.x += ahrs_dcm.gps_acceleration; // Longitudinal acceleration
#endif
accel_float.y += ahrs_dcm.gps_speed * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
accel_float.z -= ahrs_dcm.gps_speed * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
} else {
ahrs_dcm.gps_speed = 0;
ahrs_dcm.gps_acceleration = 0;
ahrs_dcm.gps_age = 100;
}
Drift_correction();
}
void estimator_update_state_analog_imu( void ) {
analog_imu_update();
/* Offset is set dynamic on Ground*/
Gyro_Vector[0]= -gyro_to_zero[G_ROLL] + gyro[G_ROLL];
Gyro_Vector[1]= -gyro_to_zero[G_PITCH] + gyro[G_PITCH];
Gyro_Vector[2]= -gyro_to_zero[G_PITCH] + gyro[G_YAW];
Accel_Vector[0] = accel[ACC_X];
Accel_Vector[1] = accel[ACC_Y];
Accel_Vector[2] = accel[ACC_Z];
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
// return euler angles to phi and theta
estimator_phi = euler.phi-imu_roll_neutral;
//estimator_phi = angle[ANG_ROLL];
estimator_theta = euler.theta-imu_pitch_neutral;
//estimator_theta = angle[ANG_PITCH];
estimator_psi = euler.psi;
}
void readImu()
{
timestamp_old = timestamp;
timestamp = millis();
if (timestamp > timestamp_old)
G_Dt = (float)(timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else G_Dt = 0;
// Update sensor readings
read_sensors();
if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) // We're in calibration mode
{
check_reset_calibration_session(); // Check if this session needs a reset
}
else if (output_mode == OUTPUT__MODE_ANGLES) // Output angles
{
// Apply sensor calibration
compensate_sensor_errors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
}
}
static void calculations_heading(void *pvParameters)
{
UARTInit(3, 115200); /* baud rate setting */
printf("Maintask: Running\n");
CLKPWR_ConfigPPWR(CLKPWR_PCONP_PCGPIO, ENABLE);
spiInit();
gyroInit();
for (;;)
{
/*Block waiting for the semaphore to become available. */
if (xSemaphoreTake( xSemaphore, 0xffff ) == pdTRUE)
{
/* It is time to execute. */;
/* Turn the LED off if it was on, and on if it was off. */
LPC_GPIO1->FIOSET = (1 << 1);
static Bool initDone = TRUE;
if (initDone)
{
static uint16_t initIteration = 0;
if (initIteration++ == 33)//not 32 since first value is dummy =0,0
{
initDone = FALSE;
imuInit_2();
}
else
imuInit_1();
}
else
{
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
calculations_motor();
static uint16_t counter = 0;
counter++;
if (counter == 4)
{
counter = 0;
xSemaphoreGive(xSemaphoreTerminal);
}
}
LPC_GPIO1->FIOCLR = (1 << 1);
}
}
}
void ahrs_update_accel(void)
{
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
// DCM filter uses g-force as positive
// accelerometer measures [0 0 -g] in a static case
accel_float.x = -accel_float.x;
accel_float.y = -accel_float.y;
accel_float.z = -accel_float.z;
#ifdef USE_GPS
if (gps.fix == GPS_FIX_3D) { //Remove centrifugal acceleration.
accel_float.y += gps.speed_3d/100. * Omega[2]; // Centrifugal force on Acc_y = GPS_speed*GyroZ
accel_float.z -= gps.speed_3d/100. * Omega[1]; // Centrifugal force on Acc_z = GPS_speed*GyroY
}
#endif
Drift_correction();
}
void ahrs_update_accel(void)
{
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
// scale accel adc raw to [m/s-2] FIXME // olri
accel_float.x /= 10.19f;
accel_float.y /= 10.5f;
accel_float.z /= 10.4f;
#ifdef USE_GPS
// Remove centrifugal acceleration.
if (gps_mode==3) {
// Centrifugal force on Acc_y = GPS_speed*GyroZ
accel_float.y += gps_speed_3d/100.f * Omega[2];
// Centrifugal force on Acc_z = GPS_speed*GyroY
accel_float.z -= gps_speed_3d/100.f * Omega[1];
}
#endif
Drift_correction();
}
int main(int argc, char**argv)
{
ros::init(argc,argv,"imu");
ros::NodeHandle n;
ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000);
ros::NodeHandle nh;
ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000);
sensor_msgs::Imu imu_msg;
std_msgs::Float64 msg;
// Read sensors, init DCM algorithm
reset_sensor_fusion();
removegyrooff();
float Y=0.0;
int i=0;
while(ros::ok())
{
// Time to read the sensors again?
if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL)
{
timestamp_old = timestamp;
timestamp = clock();
if (timestamp > timestamp_old)
G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
else G_Dt = 0;
//cout << G_Dt << endl;
// Update sensor readings
read_sensors();
// Run DCM algorithm
Compass_Heading(); // Calculate magnetic heading
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
output_angles();
imu_msg = sensor_msgs::Imu();
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "imu";
imu_msg.orientation.x = TO_DEG(pitch);
imu_msg.orientation.y = TO_DEG(roll);
imu_msg.orientation.z = TO_DEG(yaw);
imu_msg.orientation.w = 0.0;
imu_msg.orientation_covariance[0] = -1;
imu_msg.angular_velocity.x = 0.0;
imu_msg.angular_velocity.y = 0.0;
imu_msg.angular_velocity.z = 0.0;
imu_msg.angular_velocity_covariance[0] = -1;
imu_msg.linear_acceleration.x = 0.0;
imu_msg.linear_acceleration.y = 0.0;
imu_msg.linear_acceleration.z = 0.0;
imu_msg.linear_acceleration_covariance[0] = -1;
msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0]));
chatter_pub.publish(msg);
ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0])));
//ros::spinOnce();
}
}
}
// 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 vIMU_tasks()
{
uint8_t time_count;
portTickType ticks_now,ticks_old = 0;
uint8_t IMU_update_count = 0;
IMU_setup();
Baro_init();
if(RCC_GetFlagStatus(RCC_FLAG_SFTRST) == RESET) //if it was a hardware reset
{
NVIC_SystemReset(); //force software reset so that MPU sensitivity returns to normal range
}
NVIC_Configuration();
SD_Init();
//////////////////////////****************************//////////////////////////////
// create_log_file(); //Uncomment to enable log on start, else log will start only when the button on GCS is pressed
//////////////////////////****************************//////////////////////////////
for(int y=0; y<=5; y++) // Read first initial ADC values for offset.
AN_OFFSET[y] = 0;
for(;;)
{
WDT_status |= 1; //update status for IMU tasks
if(log_init_flag)
{
log_data(); //append new data to buffer
}
ticks_now = xTaskGetTickCount()*2;
if((ticks_now - ticks_old) >= 17) //do the following every 17ms
{
G_Dt = (ticks_now - ticks_old)/1000.0;
ticks_old = ticks_now;
if((!calibrating_IMU) && (!calibrating_mag)) //update IMU only if not calibrating IMU or MAG
{
IMU_Update_ARDU();
HMC5883_calculate(roll,pitch);
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();
update_variables();
}
else if(calibrating_mag) //if calibrating MAG then do not process IMU, only get raw values and obtain MAG offsets
{
if(IMU_update_count >= 5)
{
IMU_Update_ARDU();
IMU_update_count = 0;
}
else
IMU_update_count++;
}
altitude = ((Baro_update(G_Dt*1000)) - ground_alt_offset); //time in ms
innerloop(); //set servo output
if(time_count == 5) //do this every 5*17ms ~= 100ms
{
send_attitude();
send_attitude_RAW();
time_count = 0;
}
else
time_count++;
}
vTaskDelay( 17/ portTICK_RATE_MS ); //3ms less to compensate for IMU update time
}
}
