dmarkova/WoodDrone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
0de214c9a1488d02889ab269166bf9fc78d25b0a
WoodDrone/main.cpp
Dana Markova 0de214c9a1 first commit
2025-07-28 13:21:36 +03:00

567 lines
12 KiB
C++
Raw Blame History

#include <math.h>
#include <stdio.h>
#include <string.h>
#include "stm32g4xx.h"
#include "gpio.h"
#include "led.h"
#include "ori.h"
#include "bar.h"
#include "eep.h"
#include "tim.h"
#include "pwm.h"
#include "tele.h"
#include "med.h"
#include "tick.h"
#include "ibus.h"
#include "iwdg.h"
#include "pid.h"
#include "uart.h"
#include "gps.h"
#include "mag.h"
#include "tof.h"
#include "flow.h"
#include "imu.h"
#include "mot.h"
#include "move.h"
#include "prot.h"
#include "ibus.h"
#include "servo.h"
#include "crsf.hpp"
static const float PI = 3.14159265359f;
static const float TO_RAD = PI/180.0f;
#define TELE_TIMER 10 // ms
#define LED_TIMER 100 // ms
#define JOY_TIMER 200 // ms
#define LOG_TIMER 5 // ms
#define PWM_FREQ 100 // Hz
#define MAIN_UPDATE_FREQ 100 // Hz
#define MAIN_UPDATE_PRIORITY 2
PID_Data PID_X = { -1000, 1000, {-500, 500, 2.5}, {-50, 50, 0.0}, {-500, 500, 0.010} };
PID_Data PID_Y = { -1000, 1000, {-500, 500, 2.5}, {-50, 50, 0.0}, {-500, 500, 0.010} };
PID_Data PID_Z = { -1000, 1000, {-500, 500, 3}, {-50, 50, 0.0}, {-500, 500, 0.02} };
short Joy_throt = 0; // Gass
short Joy_pitch = 0; // Y
short Joy_roll = 0; // X
short Joy_yaw = 0; // Z
short Main_PWM[4]{ 0, 0, 0, 0 };
float Main_dXYZ[3]={0, 0, 0};
bool MainValid=false;
//IBUS_Data Joypad;
CRSF_Data Joypad;
bool JoypadOff;
IMU_Data DataIMU;
BAR_Data DataBAR;
MAG_Data DataMAG;
bool imu_ready=false, bar_ready=false, mag_ready=false;
void DoneProcIMU(IMU_Data& Data)
{
DataIMU=Data;
imu_ready=true;
if(bar_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}
void DoneProcBAR(bool Ready, BAR_Data& Data)
{
// if(!Ready) { BAR_GetAsunc(DoneProcBAR); return; }
DataBAR=Data;
bar_ready=true;
if(imu_ready && mag_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}
void DoneProcMAG(MAG_Data& Data)
{
DataMAG=Data;
mag_ready=true;
if(bar_ready && imu_ready) NVIC_SetPendingIRQ(TIM7_IRQn);
}
short test_pow[3];
Point DataGPS{0,0,0};
short test_p=0, test_r=0;
void Main_Update()
{
DataIMU.Bar=BAR_GetData(&DataIMU.Tmp);
IMU_Get(DataIMU);
//if(!mag_ready) MAG_GetAsunc(DoneProcMAG);
//if(!imu_ready || !bar_ready || !mag_ready) return;
//if(!imu_ready || !bar_ready) return;
PingIWDG();
imu_ready=false;
bar_ready=false;
mag_ready=false;
DataIMU.Mag={DataMAG.X, DataMAG.Y, DataMAG.Z};
ORI_Get(DataORI, MAIN_UPDATE_FREQ, DataIMU, DataBAR);
static Vec3 flow;
static long timer=0;
if(timer>=4)
{
float range=TOF_GetRange();
short dx, dy;
unsigned char qality;
bool move=FLOW_GetMotion(&dx, &dy, &qality);
flow=Motion({DataORI.SinX, DataORI.SinY, DataORI.CosZ}, move, dx, dy, range, 0);
timer=0;
if(range<0.15f)
{
Main_StatusMode=STATUS_MODE::on_ground;
Main_SysMode=SYS_MODE::hold;
}
}
else timer++;
short yaw=(DataORI.Yaw-Joy_yaw)+180;
if(yaw<0) yaw+=360;
if(yaw>360) yaw-=360;
static short pov_z;
pov_z = (short)(PID_Update(180, yaw, PID_Z, DataORI.Gyr.Z));
static float height;
static Point zero;
float altitude;
Point coord;
bool valid;
bool ready;
ready=GPS_GetCoordinates(DataGPS, valid);
valid=1;
/*MainValid=(last && valid);
if(MainValid)
{
if(Joypad.SWB>1500) { zero=coord; height=altitude; ready=true; }
GPS_LocalDistance(zero, coord, Main_dXYZ[0], Main_dXYZ[1]);
Main_dXYZ[2]=height-altitude;
}*/
//GPS_Navigation(MainValid, DataORI, Main_dXYZ);
//if(Joypad.SWA<1500 ) return;
static float heigh=0, heigh_g=0, heigh_z=0;
if(Joypad.SWA<1500)
{
PWM_SetAll(900);
Base_BeginXYZ=DataGPS;
return;
}
if(Joy_throt<1050)
{
PWM_SetAll(900);
return;
}
static bool autopilot=false;
static short throt;
throt = Joy_throt-1000;
static short pitch=0, roll=0;
if(Joypad.X>1550 || Joypad.X<1450 || Joypad.Y>1550 || Joypad.Y<1450) autopilot=false;
static float mx=0, my=0;
if(Joypad.SWD>1500)
{
MainFlyMode=MAIN_FLY_MODE::STOP;
/*if(Joypad.SWB>1500)
{
MainFlyMode=MAIN_FLY_MODE::STOP;
}
else
{
//PointGPS=DataGPS;
//PointGPS.Altitude+=3;
}*/
if(MainFlyMode==MAIN_FLY_MODE::WAIT)
{
if(autopilot==true) { PWM_SetAll(900); return; }
}
if(MainFlyMode==MAIN_FLY_MODE::START)
{
if(autopilot==true) { PWM_SetAll(1100); return; }
}
if(MainFlyMode==MAIN_FLY_MODE::STOP)
{
Main_StatusMode=STATUS_MODE::landing;
Main_SysMode=SYS_MODE::land;
if(autopilot==true)
{
//throt=500;
float shift=0.5f+flow.z;
float fx=-flow.x*shift;
float fy=-flow.y*shift;
mx+=fx;
my+=fy;
pitch=my*0.005f;
roll=mx*0.005f;
pitch+=fy*0.4f;
roll+=fx*0.4f;
if(pitch>+20) pitch=+20;
if(pitch<-20) pitch=-20;
if(roll>+20) roll=+20;
if(roll<-20) roll=-20;
Joy_pitch=pitch;
Joy_roll=roll;
static long add_power=0;
add_power=(heigh-flow.z*1000.0f)*0.1f;
if(add_power>+500) add_power=+500;
if(add_power<-500) add_power=-500;
throt+=add_power;
static int count=0;
static float last_h=0, delta_h=0;
if(count>=9)
{
delta_h=(last_h-flow.z*1000.0f);
last_h=flow.z*1000.0f;
count=0;
}
else count++;
if(delta_h>+200) delta_h=+200;
if(delta_h<-200) delta_h=-200;
throt+=delta_h*1.0f;
if(throt>800) throt=800;
if(throt<100) throt=100;
if(Joypad.SWB>1500)
{
if(flow.z>0.3f)
heigh-=3.0f;
else
heigh-=10.0f;
}
if(heigh<-5000)
{
heigh=-5000;
MainFlyMode=MAIN_FLY_MODE::START;
}
}
}
else
{
heigh=flow.z*1000.0f;
mx=0; my=0;
}
if(MainFlyMode==MAIN_FLY_MODE::FLY)
{
Main_StatusMode=STATUS_MODE::fly;
Main_SysMode=SYS_MODE::auto_mode;
if((autopilot==true) && (ready && valid))
{
//throt=500;
static float last_a=0, delta_a=0;
long power=(PointGPS.Altitude-DataGPS.Altitude)*100.0f;
static int count=0;
if(count>1)
{
delta_a=(DataGPS.Altitude-last_a);
last_a=DataGPS.Altitude;
count=0;
}
else count++;
power+=delta_a*50.0f;
if(power>+500) power=+500;
if(power<-500) power=-500;
//throt+=power;
//if(throt<100) throt=100;
}
if((autopilot==true) && (ready && valid))
{
static float delta_x=0, delta_y=0;
static float ldx=0, ldy=0;
float dx, dy;
GPS_LocalDistance(PointGPS, DataGPS, dx, dy);
dx=dx;
dy=-dy;
pitch=dy*10.0f;
roll=dx*10.0f;
if(pitch>+20) pitch=+20;
if(pitch<-20) pitch=-20;
if(roll>+20) roll=+20;
if(roll<-20) roll=-20;
static int count=0;
if(count>1)
{
delta_x=(delta_x+(dx-ldx))/2.0f;
delta_y=(delta_y+(dy-ldy))/2.0f;
ldx=dx;
ldy=dy;
count=0;
}
else count++;
//pitch+=delta_y*10.0f;
//roll+=delta_x*10.0f;
if(pitch>+20) pitch=+20;
if(pitch<-20) pitch=-20;
if(roll>+20) roll=+20;
if(roll<-20) roll=-20;
test_p=pitch;
test_r=roll;
//Vec3 v = RotateToZ({(float)pitch, (float)roll, 0}, false);
//pitch=v.x; roll=v.y;
Joy_pitch=pitch;
Joy_roll=roll;
}
if(autopilot==true)
{
Joy_pitch=test_p;
Joy_roll=test_r;
}
}
}
else
{
mx=0; my=0;
heigh=flow.z*1000.0f;
MainFlyMode=MAIN_FLY_MODE::WAIT;
autopilot=true;
}
short pov_x = (short)(PID_Update(Joy_pitch, DataORI.Pitch, PID_X, -DataORI.Gyr.X));
short pov_y = (short)(PID_Update(Joy_roll, DataORI.Roll, PID_Y, -DataORI.Gyr.Y));
//throt+=pov_h;
//float stream=DataORI.Tilt;
//if(stream>25) stream=25;
//throt*=1.0f/cosf(stream*TO_RAD);
if(throt>1000) throt=1000;
QUAD_Set2(throt, pov_x, pov_y, pov_z);
}
//------------------------------------------------------------------------------
bool SWAconnect = false;
int main()
{
//InitIWDG(1000);
GPIO_InitPin(GPIO_PIN_13 | GPIO_PORT_C | GPIO_OUTPUT); // POWER ON 3V3
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
DWT->CYCCNT = 0;
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
//asm("BKPT 0");
PWM_Init(PWM_FREQ); // 100Hz
PWM_SetAll(900);
IMU_Init();
LED_Init();
ORI_Init();
BAR_Init();
EEP_Init();
TIM7_Init(MAIN_UPDATE_PRIORITY);
//TELE_Init();
TICK_Init();
//IBUS_Init(115200); // 115200
CRSF_Init();
GPS_Init();
//MAG_Init();
TOF_Init();
LPUART1_Init(57600);
FLOW_Init();
//EEP_Write(0, "Hello", 6);
char data[32];
EEP_Read(2, data, 32);
//LED_Red_Set(true);
//InitIWDG(300);
Servo_init(100);
static unsigned short ServoOpen = 1900, ServoClose = 2350;
Servo_update(ServoClose);
TIM7_Update(MAIN_UPDATE_FREQ*2, Main_Update);
while(true)
{
GPS_Update();
TOF_Update2();
PROT_Update();
const unsigned long tick=TICK_GetCount();
static unsigned long joy_tick=tick;
if(CRSF_Update(Joypad, JoypadOff))
{
joy_tick=tick;
if(Joypad.SWA>1500)
{
if (!SWAconnect)
{
QuatGoToZero();
SWAconnect = true;
}
Joy_pitch = (1500 - Joypad.Y)/16;
Joy_roll = (Joypad.X - 1500)/16;
Joy_yaw = (Joypad.W - 1500)/16;
if (Joypad.Z < 1050) Joy_throt = 1000;
else if (Joypad.Z >= 1050 && Joypad.Z <= 2000)
{
Joy_throt = Joypad.Z;
}
}
else
{
PWM_SetAll(900);
SWAconnect = false;
}
if (Joypad.SWC<1500)
Servo_update(ServoClose);
else
Servo_update(ServoOpen);
}
if((tick-joy_tick>JOY_TIMER) || JoypadOff)
{
PWM_SetAll(900);
}
static unsigned long led_tick=0;
static bool led=false;
if(tick-led_tick>LED_TIMER)
{
led_tick=tick;
LED_Set(led=!led);
}
//short info[]={DataORI.Acc.X, DataORI.Acc.Y, DataORI.Acc.Z, DataORI.Acc.Sum, DataORI.Gyr.X, DataORI.Gyr.Y, DataORI.Gyr.Z, (short)DataORI.Pitch, (short)DataORI.Roll, (short)DataORI.Yaw};
//TELE_Update(info, sizeof(info), TELE_TIMER);
static int Fixed=0;
static unsigned long log_tick=0;
if(tick-log_tick>TELE_TIMER)
{
log_tick=tick;
float info[]={DataORI.Bar.Acc, DataORI.Speed.Z, DataORI.Bar.Speed};
UART2_Send("Z", 1);
UART2_Send(info, sizeof(info));
UART2_Send("V", 1);
static char buffer[128];
//int len=sprintf(buffer, "Z:%d,%d,%d,%d,%d,%d,%d#\n", Fixed, (int)DataORI.Pitch, (int)DataORI.Roll, (int)DataORI.Yaw, (int)(DataORI.Iner.X*1000), (int)(DataORI.Iner.Y*1000), (int)(DataORI.Iner.Z*1000));
//if(len>0) LPUART1_Send(buffer, len);
//int len=sprintf(buffer, "G:0,%4.3f,%4.8f,%4.8f#\n", DataGPS.Altitude, DataGPS.Coord.Latitude, DataGPS.Coord.Longitude);
//int len=sprintf(buffer, "G:0,%d,%d\n", test_p, test_r);
//if(len>0) LPUART1_Send(buffer, len);
Fixed++;
}
}
}
//------------------------------------------------------------------------------
Reference in New Issue View Git Blame Copy Permalink