189 lines
3.8 KiB
C++
189 lines
3.8 KiB
C++
#include "stm32g4xx.h"
|
|
#include "Tick.h"
|
|
#include "Tim.h"
|
|
#include "IIMU.h"
|
|
#include "ICM20948.h"
|
|
#include "IRS.h"
|
|
#include "Vector.h"
|
|
#include "Autopilot.h"
|
|
#include "RadioReceiver.h"
|
|
#include "Motors.h"
|
|
#include "Attitude.h"
|
|
|
|
#define PI 3.14159265359f
|
|
|
|
extern "C" void SystemClock_Config();
|
|
|
|
constexpr long TIM_PRIORITY = 2;
|
|
|
|
IMU_Data DataIMU;
|
|
|
|
IRS MainIRS;
|
|
|
|
IIMU* IMUObj;
|
|
|
|
float GetCurrentTime()
|
|
{
|
|
return ((float)TICK_GetCount()) / 1000.0f;
|
|
}
|
|
|
|
Vector3 QuatToEuler(const Quaternion& q)
|
|
{
|
|
Vector3 e;
|
|
|
|
e.X = atan2f(2*(q.W*q.X + q.Y*q.Z), 1 - 2*(q.X*q.X + q.Y*q.Y));
|
|
|
|
e.Y = asinf(2*(q.W*q.Y - q.Z*q.X));
|
|
|
|
e.Z = atan2f(2*(q.W*q.Z + q.X*q.Y), 1 - 2*(q.Y*q.Y + q.Z*q.Z));
|
|
|
|
e.X *= 180.0f / PI;
|
|
e.Y *= 180.0f / PI;
|
|
e.Z *= 180.0f / PI;
|
|
|
|
return e;
|
|
}
|
|
|
|
void DoneProcIMU(IMU_Data& Data)
|
|
{
|
|
DataIMU = Data;
|
|
// MagReady=true;
|
|
}
|
|
|
|
void TimerUpdateSensor()
|
|
{
|
|
//IMUObj->GetAsync(DoneProcIMU);
|
|
if(MainDrone.IMUInit) IMUObj->GetAsync(DoneProcIMU);
|
|
//if(MainDrone.BarInit) BarObj->GetAsync(DoneProcBAR);
|
|
}
|
|
|
|
Vector3 Euler;
|
|
|
|
void TimerUpdateMain()
|
|
{
|
|
float time = GetCurrentTime();
|
|
|
|
Vector3 gyr = {(float)DataIMU.Gyr.X, (float)DataIMU.Gyr.Y, (float)DataIMU.Gyr.Z};
|
|
Vector3 acc = {(float)DataIMU.Acc.X, (float)DataIMU.Acc.Y, (float)DataIMU.Acc.Z};
|
|
|
|
// Vector3 mag;
|
|
// if(MainDrone.ExternMagInit) mag = {(float)DataMAG.X, (float)DataMAG.Y, (float)DataMAG.Z};
|
|
// else mag = {(float)DataIMU.Mag.X, (float)DataIMU.Mag.Y, (float)DataIMU.Mag.Z};
|
|
|
|
MainIRS.UpdateGyro(gyr);
|
|
MainIRS.UpdateAccel(acc);
|
|
|
|
Euler = QuatToEuler(MainIRS.OriQuat);
|
|
|
|
/*if(MagReady)
|
|
{
|
|
MagReady=false;
|
|
MainIRS.UpdateMagnet(mag);
|
|
}
|
|
|
|
MainGPS.UpdateAverage(time);
|
|
MainBar.UpdateAverage();
|
|
MainOF.UpdateAverage(IRS::Period);
|
|
MainLen.UpdateAverage();
|
|
|
|
if(DataFLOW.Update)
|
|
{
|
|
DataFLOW.Update=false;
|
|
UpdateFlow(DataFLOW.OF, MainIRS.Inertial.Pos.Z - MainIRS.GroundShift, DataFLOW.Valid, DataFLOW.Quality);
|
|
}
|
|
|
|
if(DataTOF.Update)
|
|
{
|
|
DataTOF.Update=false;
|
|
UpdateRange(DataTOF.Range, DataTOF.Valid && DataTOF.Range>0.02f && DataTOF.Strength>200.0f);
|
|
}
|
|
|
|
UpdateBar(DataBAR.Pressure, DataBAR.Temp, DataBAR.Pressure>0.0f);
|
|
|
|
if(DataGPS.Update)
|
|
{
|
|
DataGPS.Update=false;
|
|
Vector3 gps = { DataGPS.X, DataGPS.Y, DataGPS.Z };
|
|
UpdateGPS(gps, time, DataGPS.Valid);
|
|
}*/
|
|
|
|
Vector3 pos;
|
|
MainIRS.RestoreAllShift(pos);
|
|
/*MainVel.PointBegin-=pos;
|
|
|
|
AutoControl(gyr);
|
|
|
|
ReadStateINS();
|
|
|
|
if(MainDrone.ExternMagInit) MagObj->GetAsync(DoneProcMag);*/
|
|
}
|
|
|
|
static bool control_update_flag = false;
|
|
|
|
void TimerUpdateControl()
|
|
{
|
|
control_update_flag = true;
|
|
}
|
|
|
|
int main()
|
|
{
|
|
SystemClock_Config(); // 170MHz
|
|
IMU_InitPower();
|
|
|
|
|
|
|
|
TICK_Init();
|
|
|
|
TIM6_Init(TIM_PRIORITY, 500, TimerUpdateSensor, TimerUpdateMain);
|
|
TIM7_Init(TIM_PRIORITY, 200, TimerUpdateControl);
|
|
|
|
IMUObj = TryFindIMU(MainDrone.IMUInit);
|
|
|
|
TIM6_Enable();
|
|
TIM7_Enable();
|
|
|
|
static attitude_t attitude;
|
|
static rc_channels rx_chs_raw;
|
|
static rc_channels rx_chs_normalized;
|
|
static control_channels_t ctrl_chs;
|
|
|
|
attitude_init(&attitude);
|
|
receiver_init();
|
|
motors_init();
|
|
|
|
while (true)
|
|
{
|
|
receiver_update(&rx_chs_raw);
|
|
rx_chs_normalized = normalize_channels(rx_chs_raw);
|
|
|
|
if (control_update_flag)
|
|
{
|
|
control_update_flag = false;
|
|
|
|
Vector3 Gyro = Vector3(DataIMU.Gyr.X, DataIMU.Gyr.Y, DataIMU.Gyr.Z);
|
|
|
|
attitude_controller_update(
|
|
&ctrl_chs,
|
|
&rx_chs_normalized,
|
|
&MainIRS.OriQuat,
|
|
&Gyro
|
|
);
|
|
}
|
|
|
|
if (rx_chs_normalized.rc_armed)
|
|
{
|
|
CalibDataIMU.AllowedCalib = false;
|
|
|
|
motors_set_throttle_mix(
|
|
rx_chs_normalized.rc_throttle,
|
|
&ctrl_chs,
|
|
rx_chs_normalized.rc_armed
|
|
);
|
|
}
|
|
else
|
|
{
|
|
motors_turn_off();
|
|
CalibDataIMU.AllowedCalib = true;
|
|
}
|
|
}
|
|
} |