add firmware
This commit is contained in:
Dana Markova
587
main.cpp
Normal file
587
main.cpp
Normal file
@ -0,0 +1,587 @@
|
||||
#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 "sbus.h"
|
||||
#include "iwdg.h"
|
||||
#include "pid.h"
|
||||
#include "uart.h"
|
||||
#include "gps.h"
|
||||
|
||||
#include "laser.h"
|
||||
#include "flow.h"
|
||||
|
||||
#include "imu.h"
|
||||
|
||||
#include "mot.h"
|
||||
#include "move.h"
|
||||
|
||||
#include "prot.h"
|
||||
|
||||
|
||||
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, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
|
||||
PID_Data PID_Y = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.020} };
|
||||
PID_Data PID_Z = { -1000, 1000, {-500, 500, 1}, {-50, 50, 0.0}, {-500, 500, 0.03} };
|
||||
|
||||
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;
|
||||
SBUS_Data Joypad;
|
||||
bool JoypadOff;
|
||||
|
||||
IMU_Data DataIMU;
|
||||
BAR_Data DataBAR;
|
||||
MAG_Data DataMAG = {0,0,0};
|
||||
|
||||
|
||||
bool imu_ready=false, bar_ready=false, mag_ready=false;
|
||||
|
||||
|
||||
|
||||
SBUS_Data rc;
|
||||
unsigned char fs;
|
||||
|
||||
short PWM_NONE = 0;
|
||||
|
||||
|
||||
int m= 0;
|
||||
unsigned short dist_mm;
|
||||
uint16_t dist ;
|
||||
float meters;
|
||||
|
||||
short dx, dy;
|
||||
unsigned char qality;
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
short test_pow[3];
|
||||
|
||||
Point DataGPS{0,0,0};
|
||||
|
||||
short test_p=0, test_r=0;
|
||||
|
||||
Vec3 test_iner={0,0,0};
|
||||
|
||||
float range = 0;
|
||||
|
||||
void Main_Update()
|
||||
{
|
||||
IMU_Get(DataIMU);
|
||||
|
||||
DataIMU.Bar=BAR_GetData(&DataIMU.Tmp);
|
||||
//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)
|
||||
{
|
||||
if (isRangeReady())
|
||||
{
|
||||
range = getRange();
|
||||
}
|
||||
|
||||
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>JOY_MID) { 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<JOY_MID ) return;
|
||||
|
||||
static float heigh=0, heigh_g=0, heigh_z=0;
|
||||
|
||||
if(Joypad.SWA<JOY_MID)
|
||||
{
|
||||
PWM_SetAll(PWM_NONE);
|
||||
Base_BeginXYZ=DataGPS;
|
||||
return;
|
||||
}
|
||||
|
||||
if(Joy_throt<JOY_MIN+50)
|
||||
{
|
||||
PWM_SetAll(PWM_NONE);
|
||||
return;
|
||||
}
|
||||
|
||||
static bool autopilot=false;
|
||||
|
||||
static short throt;
|
||||
throt = (Joy_throt-JOY_MIN)/JOY_VAL;
|
||||
|
||||
static short pitch=0, roll=0;
|
||||
|
||||
|
||||
|
||||
if(Joypad.X>JOY_MID+50 || Joypad.X<JOY_MID-50 || Joypad.Y>JOY_MID+50 || Joypad.Y<JOY_MID-50) autopilot=false;
|
||||
|
||||
static float mx=0, my=0;
|
||||
|
||||
if(Joypad.SWD>JOY_MID)
|
||||
{
|
||||
MainFlyMode=MAIN_FLY_MODE::STOP;
|
||||
|
||||
/*if(Joypad.SWB>JOY_MID)
|
||||
{
|
||||
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;
|
||||
|
||||
test_iner.x=mx/1000;
|
||||
test_iner.y=my/1000;
|
||||
test_iner.z=flow.z;
|
||||
|
||||
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>JOY_MID)
|
||||
{
|
||||
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_Set(throt, pov_x, pov_y, pov_z); //!!!!!!!!!!!!!!!!
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
float poww=0;
|
||||
bool isGoZero = false;
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
//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); //???
|
||||
PWM_SetAll(PWM_NONE);//???
|
||||
|
||||
LED_Init();
|
||||
|
||||
|
||||
ORI_Init();
|
||||
BAR_Init();
|
||||
|
||||
// EEP_Init();
|
||||
TIM7_Init(MAIN_UPDATE_PRIORITY);
|
||||
TICK_Init();
|
||||
SBUS_Init();
|
||||
Laser_Init_v2();
|
||||
UART2_Init(57600);//LPUART1_Init
|
||||
|
||||
FLOW_Init();
|
||||
//EEP_Write(0, "Hello", 6);
|
||||
//char data[32];
|
||||
//EEP_Read(2, data, 32);
|
||||
|
||||
TIM7_Update(MAIN_UPDATE_FREQ*2, Main_Update);
|
||||
|
||||
//174 1811
|
||||
while(true)
|
||||
{
|
||||
// GPS_Update();
|
||||
|
||||
/*
|
||||
if(SBUS_Update(Joypad, JoypadOff))
|
||||
{
|
||||
poww = (Joypad.Z-174)*700/(1811-174);
|
||||
PWM_SetAll(PWM_NONE);
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
PROT_Update();
|
||||
|
||||
const unsigned long tick=TICK_GetCount();
|
||||
|
||||
static unsigned long joy_tick=tick;
|
||||
|
||||
|
||||
|
||||
|
||||
//--------------------------------------------------
|
||||
|
||||
if(SBUS_Update(Joypad, JoypadOff))
|
||||
{
|
||||
joy_tick=tick;
|
||||
if(Joypad.SWA>JOY_MID)
|
||||
{
|
||||
if (!isGoZero)
|
||||
{
|
||||
GoZero();
|
||||
isGoZero = true;
|
||||
}
|
||||
Joy_pitch = (JOY_MID_PITCH - Joypad.Y)/16;
|
||||
Joy_roll = (Joypad.X - JOY_MID_ROLL)/16;
|
||||
Joy_yaw = (Joypad.W - JOY_MID_YAW)/16;
|
||||
|
||||
if (Joypad.Z < JOY_MIN+50) Joy_throt = JOY_MIN;
|
||||
else if (Joypad.Z >= JOY_MIN+50 && Joypad.Z <= JOY_MAX)
|
||||
{
|
||||
Joy_throt = Joypad.Z;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
PWM_SetAll(PWM_NONE);
|
||||
isGoZero = false;
|
||||
}
|
||||
}
|
||||
|
||||
//--------------------------------------------------
|
||||
if((tick-joy_tick>JOY_TIMER) || JoypadOff)
|
||||
{
|
||||
PWM_SetAll(PWM_NONE);
|
||||
}
|
||||
|
||||
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
Block a user