Vik321:
stm32f103
Then you do not need the loop. Imagine, if you will, using an empty loop:
#include <STM32FreeRTOS.h> // works. stm32 boards Generic STM32F103 series (selected from submenu)
#include <MPU9250.h>
/* https://github.com/bolderflight/MPU9250 */
MPU9250 IMU( SPI, PB0 );
//////////
/*Pin connections
STM32 MPU9250
3.3 Vcc
GND GND
PA5 SCK(SCL)
PA7 MOSI(SDA)
PA6 MISO(ADO)
PB0 NCS
GND FSYNC*/
//////////
void setup() {
Serial.begin(115200);
//
pinMode(PC13, OUTPUT);
//
IMU.begin();
// set the accelerometer full scale range to the given value
// IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
// set the gyroscope full scale range to the given value.
// IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
// setting DLPF bandwidth. Digital Low Pass Filter (DLPF) bandwidth. fast bandwidth faster gryo reacts
//// DLPF_BANDWIDTH_184HZ DLPF_BANDWIDTH_92HZ DLPF_BANDWIDTH_41HZ DLPF_BANDWIDTH_20HZ DLPF_BANDWIDTH_10HZ DLPF_BANDWIDTH_5HZ
// IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_92HZ); // defaults to DLPF_BANDWIDTH_184HZ
//// setting SRD to 1 for a 500 Hz update rate, 1000/(1+1)
IMU.setSrd(1);
// Task function, name of task, Stack size of task, parameter of the task, priority of the task, Task handle to keep track of created task
xTaskCreate(v_getIMU, "getIMU", 2100, NULL, tskIDLE_PRIORITY + 2, NULL);
xTaskCreate(fBlinkBuiltIn, "fBlinkBuiltIn", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY + 1, NULL);
// start task
vTaskStartScheduler();
}
//
void loop() {} // <<<<< SEE! Empty.
//
// mpu should be level and held still for this process
void calibrate_sensors(float RADIANS_TO_DEGREES, float& xOffset, float& yOffest )
{
int num_readings = 10;
// Discard the first reading
IMU.readSensor();
vTaskDelay( pdMS_TO_TICKS( 1 ) );
//
IMU.calibrateAccel();
//
IMU.setAccelCalX( IMU.getAccelBiasX_mss(), IMU.getAccelScaleFactorX() );
IMU.setAccelCalY( IMU.getAccelBiasY_mss(), IMU.getAccelScaleFactorY() );
IMU.setAccelCalZ( IMU.getAccelBiasZ_mss(), IMU.getAccelScaleFactorZ() );
//
IMU.calibrateGyro();
//
IMU.setGyroBiasX_rads( IMU.getGyroBiasX_rads() );
IMU.setGyroBiasY_rads( IMU.getGyroBiasY_rads() );
IMU.setGyroBiasZ_rads( IMU.getGyroBiasZ_rads() );
// Read and average the raw values
for (int i = 0; i < num_readings; i++)
{
if ( IMU.readSensor() > -1 )
{
// get the data
// calculate angle based on acceleration, store
yOffest += atan(-1 * IMU.getAccelX_mss() / sqrt(pow(IMU.getAccelY_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
xOffset += atan(IMU.getAccelY_mss() / sqrt(pow(IMU.getAccelX_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
//
}
else // if readsensor fail decrement i if i is not zero
{
if ( i != 0 )
{
i -= 1;
}
}
vTaskDelay( pdMS_TO_TICKS(2) );
}
yOffest /= num_readings; // average the readings
xOffset /= num_readings;
} // void calibrate_sensors()
//
//
static void v_getIMU( void *pvParameters )
{
const float RADIANS_TO_DEGREES = 57.2958; //180/3.14159
const float K = 0.97; //this is the gyro:accelerometer ratio
const float K1 = 1 - K;
const TickType_t xDelay = pdMS_TO_TICKS(1);
float accel_angle_y;
float accel_angle_x;
float angle_z = 0.0;
float last_x_angle;
float last_y_angle;
float dt;
unsigned long t_now = micros(); // stem32 millis record 0, use micros instead
unsigned long last_read_time = micros();
// unsigned long t_start = micros();
float angle_x;
float angle_y;
float base_accel_angle_x = 0;
float base_accel_angle_y = 0;
float TourqeAngle_uSec = 22.22; // 22.22uS per degree, based upon set servo limits of 90 degrees (+/-45 degrees)
int XservoInitial = 1500; // may need to adjust to servo mounted level.
int YservoInitial = 1500;
String sBuffer;
sBuffer.reserve( 25 );
calibrate_sensors(RADIANS_TO_DEGREES, base_accel_angle_x, base_accel_angle_y );
//
for (;;)
{
// read the sensor
if ( IMU.readSensor() > -1 )
{
//t_start = micros(); // remove in final version
t_now = micros();
// Get time of last data read
dt = (t_now - last_read_time) / 1000000.0f;
// calculate accelerometer angles
accel_angle_y = atan(-1 * IMU.getAccelX_mss() / sqrt(pow(IMU.getAccelY_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
accel_angle_x = atan(IMU.getAccelY_mss() / sqrt(pow(IMU.getAccelX_mss(), 2) + pow(IMU.getAccelZ_mss(), 2))) * RADIANS_TO_DEGREES;
accel_angle_y -= base_accel_angle_y; //subtract angle offset from angle reading, accelerometer
accel_angle_x -= base_accel_angle_x;
// Compute the (filtered) gyro angles
angle_x = IMU.getGyroX_rads() * dt + last_x_angle; // factor in offset here if needed
angle_y = IMU.getGyroY_rads() * dt + last_y_angle;
// Apply the complementary filter
angle_x = K * angle_x + (K1 * accel_angle_x);
angle_y = K * angle_y + (K1 * accel_angle_y);
if ( (abs(angle_x) > 0.1) || (abs(angle_y) > 0.1) )
{
float xMagnitude = angle_x * TourqeAngle_uSec;
float yMagnitude = angle_y * TourqeAngle_uSec;
float xAdjustment = XservoInitial - xMagnitude; // may need to adjust sign depending on servo/mpu orientation for proper direction
float yAdjustment = YservoInitial - yMagnitude; // may need to adjust sign depending on servo/mpu orientation for proper direction
//set high low limit
// send xAdjustment and yAdjustment torques to servos
// print the data for testing
sBuffer.concat( String(angle_x) );
sBuffer.concat( "," );
sBuffer.concat( String(xAdjustment) );
sBuffer.concat( "," );
sBuffer.concat( String(angle_y) );
sBuffer.concat( "," );
sBuffer.concat( String(yAdjustment) );
// sBuffer.concat( String(sBuffer.length()) );
sBuffer.concat( "," );
// Serial.print( sBuffer );
// Serial.print(uxTaskGetStackHighWaterMark( NULL )); // stack size used
// Serial.println();
// Serial.flush();
sBuffer = "";
}
// Serial.print( micros() - t_now ); // time of execution
// Serial.println();
} // if ( IMU.readSensor() > -1 )
// Update the saved data with the latest values
last_read_time = t_now;
last_x_angle = angle_x;
last_y_angle = angle_y;
//
vTaskDelay( xDelay );
}
vTaskDelete( NULL );
}
//
void fBlinkBuiltIn( void* pvParameters )
{
// toggle built in LED off/on
for (;;)
{
digitalWrite(PC13, LOW);
vTaskDelay( pdMS_TO_TICKS( 10 ) );
digitalWrite(PC13, HIGH);
vTaskDelay( pdMS_TO_TICKS( 1000 ) );
}
vTaskDelete( NULL );
}
//