587 lines
12 KiB
C++
587 lines
12 KiB
C++
#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++;
|
|
}
|
|
}
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
|