int main(void)
{
uint32_t ppg; //PPG channel
uint32_t data_counter=0; //used as data timestamp
uint8_t system_state=0; //used to track button press functionality
float sensor_data; //used for handling data passed back from sensors
RTC_t RTC_time;
_REENT_INIT_PTR(&my_reent);
_impure_ptr = &my_reent;
SystemInit(); //Sets up the clk
setup_gpio(); //Initialised pins, and detects boot source
DBGMCU_Config(DBGMCU_IWDG_STOP, ENABLE); //Watchdog stopped during JTAG halt
if(RCC->CSR&RCC_CSR_IWDGRSTF) { //Watchdog reset, turn off
RCC->CSR|=RCC_CSR_RMVF; //Reset the reset flags
shutdown();
}
SysTick_Configuration(); //Start up system timer at 100Hz for uSD card functionality
Watchdog_Config(WATCHDOG_TIMEOUT); //Set the watchdog
Watchdog_Reset(); //Reset watchdog as soon as possible incase it is still running at power on
rtc_init(); //Real time clock initialise - (keeps time unchanged if set)
Usarts_Init();
ISR_Config();
rprintfInit(__usart_send_char); //Printf over the bluetooth
if(USB_SOURCE==bootsource) {
Set_System(); //This actually just inits the storage layer
Set_USBClock();
USB_Interrupts_Config();
USB_Init();
uint32_t nojack=0x000FFFFF; //Countdown timer - a few hundered ms of 0v on jack detect forces a shutdown
while (bDeviceState != CONFIGURED) { //Wait for USB config - timeout causes shutdown
if(Millis>10000 || !nojack) //No USB cable - shutdown (Charger pin will be set to open drain, cant be disabled without usb)
shutdown();
if(GET_VBUS_STATE) //Jack detect resets the countdown
nojack=0x0FFFFF;
nojack--;
Watchdog_Reset(); //Reset watchdog here, if we are stalled here the Millis timeout should catch us
}
USB_Configured_LED();
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
while(1) { //If running off USB (mounted as mass storage), stay in this loop - dont turn on anything
if(Millis%1000>500) //1Hz on/off flashing
switch_leds_on(); //Flash the LED(s)
else
switch_leds_off();
Watchdog_Reset();
}
}
if(!GET_PWR_STATE) //Check here to make sure the power button is still pressed, if not, sleep
shutdown(); //This means a glitch on the supply line, or a power glitch results in sleep
for(uint8_t n=0;n<PPG_CHANNELS;n++)
init_buffer(&(Buff[n]),PPG_BUFFER_SIZE);//Enough for ~0.25S of data
setup_pwm(); //Enable the PWM outputs on all three channels
Delay(100000); //Sensor+inst amplifier takes about 100ms to stabilise after power on
ADC_Configuration(); //We leave this a bit later to allow stabilisation
calibrate_sensor(); //Calibrate the offset on the diff pressure sensor
EXTI_ONOFF_EN(); //Enable the off interrupt - allow some time for debouncing
I2C_Config(); //Setup the I2C bus
uint8_t sensors_=detect_sensors(); //Search for connected sensors
sensor_data=GET_BATTERY_VOLTAGE; //Have to flush adc for some reason
Delay(10000);
if(!(sensors_&~(1<<PRESSURE_HOSE))||GET_BATTERY_VOLTAGE<BATTERY_STARTUP_LIMIT) {//We will have to turn off
Watchdog_Reset(); //LED flashing takes a while
if(abs(Reported_Pressure)>PRESSURE_MARGIN)
Set_Motor(-1); //If the is air backpressure, dump to rapidly drop to zero pressure before turnoff
if(file_opened)
f_close(&FATFS_logfile); //be sure to terminate file neatly
red_flash();
Delay(400000);
red_flash(); //Two flashes means battery abort -----------------ABORT 2
if(sensors_&~(1<<PRESSURE_HOSE))
shutdown();
Delay(400000);
red_flash(); //Three flashes means no sensors abort ------------ABORT 3
shutdown();
}
if((f_err_code = f_mount(0, &FATFS_Obj)))Usart_Send_Str((char*)"FatFs mount error\r\n");//This should only error if internal error
else { //FATFS initialised ok, try init the card, this also sets up the SPI1
if(!f_open(&FATFS_logfile,"time.txt",FA_OPEN_EXISTING | FA_READ | FA_WRITE)) {//Try and open a time file to get the system time
if(!f_stat((const TCHAR *)"time.txt",&FATFS_info)) {//Get file info
if(!FATFS_info.fsize) { //Empty file
RTC_time.year=(FATFS_info.fdate>>9)+1980;//populate the time struct (FAT start==1980, RTC.year==0)
RTC_time.month=(FATFS_info.fdate>>5)&0x000F;
RTC_time.mday=FATFS_info.fdate&0x001F;
RTC_time.hour=(FATFS_info.ftime>>11)&0x001F;
RTC_time.min=(FATFS_info.ftime>>5)&0x003F;
RTC_time.sec=(FATFS_info.ftime<<1)&0x003E;
rtc_settime(&RTC_time);
rprintfInit(__fat_print_char);//printf to the open file
printf("RTC set to %d/%d/%d %d:%d:%d\n",RTC_time.mday,RTC_time.month,RTC_time.year,\
RTC_time.hour,RTC_time.min,RTC_time.sec);
}
}
f_close(&FATFS_logfile); //Close the time.txt file
}
#ifndef SINGLE_LOGFILE
rtc_gettime(&RTC_time); //Get the RTC time and put a timestamp on the start of the file
rprintfInit(__str_print_char); //Print to the string
printf("%d-%d-%dT%d-%d-%d.txt",RTC_time.year,RTC_time.month,RTC_time.mday,RTC_time.hour,RTC_time.min,RTC_time.sec);//Timestamp name
rprintfInit(__usart_send_char); //Printf over the bluetooth
#endif
if((f_err_code=f_open(&FATFS_logfile,LOGFILE_NAME,FA_CREATE_ALWAYS | FA_WRITE))) {//Present
printf("FatFs drive error %d\r\n",f_err_code);
if(f_err_code==FR_DISK_ERR || f_err_code==FR_NOT_READY)
Usart_Send_Str((char*)"No uSD card inserted?\r\n");
}
else { //We have a mounted card
f_err_code=f_lseek(&FATFS_logfile, PRE_SIZE);// Pre-allocate clusters
if (f_err_code || f_tell(&FATFS_logfile) != PRE_SIZE)// Check if the file size has been increased correctly
Usart_Send_Str((char*)"Pre-Allocation error\r\n");
else {
if((f_err_code=f_lseek(&FATFS_logfile, 0)))//Seek back to start of file to start writing
Usart_Send_Str((char*)"Seek error\r\n");
else
rprintfInit(__str_print_char);//Printf to the logfile
}
if(f_err_code)
f_close(&FATFS_logfile);//Close the already opened file on error
else
file_opened=1; //So we know to close the file properly on shutdown
}
}
void CONTROL1(float rol_now, float pit_now, float yaw_now, u16 throttle, float rol_tar, float pit_tar, s16 yaw_gyro_tar,s16* ACC,s16* GYRO)
{
////////////////////////外环角度环(PID)///////////////////////////////
Pitch_i+=(pit_now-pit_tar);
//-------------Pitch积分限幅----------------//
if(Pitch_i>300) Pitch_i=300;
else if(Pitch_i<-300) Pitch_i=-300;
//-------------Pitch微分--------------------//
Pitch_d=pit_now-Pitch_old;
//-------------Pitch PID-------------------//
Pitch_shell_out = Pitch_shell_kp*(pit_now-pit_tar) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
//角度保存
Pitch_old=pit_now;
/*********************************************************/
Roll_i+=(rol_now-rol_tar);
//-------------Roll积分限幅----------------//
if(Roll_i>300) Roll_i=300;
else if(Roll_i<-300) Roll_i=-300;
//-------------Roll微分--------------------//
Roll_d=rol_now-Roll_old;
//-------------Roll PID-------------------//
Roll_shell_out = Roll_shell_kp*(rol_now-rol_tar) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
//------------Roll角度保存------------------//
Roll_old=rol_now;
//-------------Yaw微分--------------------//
Yaw_d=GYRO[2]-Yaw_old;
//-------------Roll PID-------------------//
Yaw_shell_out = Yaw_shell_kp*(GYRO[2]-yaw_gyro_tar) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
//------------Roll角度保存------------------//
Yaw_old=GYRO[2];
////////////////////////内环角速度环(PD)///////////////////////////////
pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + GYRO[1]* 3.5);
pitch_core_kd_out = Pitch_core_kd * (GYRO[1] - Gyro_radian_old_y);
Roll_core_kp_out = Roll_core_kp * (Roll_shell_out + GYRO[0] *3.5);
Roll_core_kd_out = Roll_core_kd * (GYRO[0] - Gyro_radian_old_x);
Yaw_core_kp_out = Yaw_core_kp * (Yaw_shell_out + GYRO[2] * 1);
Yaw_core_kd_out = Yaw_core_kd * (GYRO[2] - Gyro_radian_old_z);
Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
Roll_core_out = Roll_core_kp_out + Roll_core_kd_out;
Yaw_core_out = Yaw_core_kp_out + Yaw_core_kd_out;
Gyro_radian_old_y = GYRO[1];
Gyro_radian_old_x = GYRO[0];
Gyro_radian_old_z = GYRO[2]; //储存历史值
//--------------------将输出值融合到四个电机--------------------------------//
if(throttle>=1100)//对油门进行进行判断<1100的油门电机不转动！！！！！(为了安全很重要！！！)
{
moto1 = throttle + Roll_core_out - Pitch_core_out + Yaw_core_out; moto1 = Get_MxMi1(moto1,2000,1000);
moto2 = throttle - Roll_core_out - Pitch_core_out - Yaw_core_out; moto2 = Get_MxMi1(moto2,2000,1000);
moto3 = throttle - Roll_core_out + Pitch_core_out + Yaw_core_out; moto3 = Get_MxMi1(moto3,2000,1000);
moto4 = throttle + Roll_core_out + Pitch_core_out - Yaw_core_out; moto4 = Get_MxMi1(moto4,2000,1000);
Set_Motor(moto1,moto2,moto3,moto4);//电机PWM输出
}
else
{
moto1 =1000;moto2 =1000;moto3 =1000;moto4 =1000;
Set_Motor(moto1,moto2,moto3,moto4);//电机PWM输出
}
}
/*******************************************************************************
* Function Name : SysTickHandler
* Description : This function handles SysTick Handler - runs at 100hz.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
__attribute__((externally_visible)) void SysTickHandler(void)
{
static float I,old_pressure;
static uint16_t Enabled_iterations; //Note, this is going to break if we spend long periods with +ive pressure set
static uint32_t Last_Button_Press; //Holds the timestamp for the previous button press
static uint8_t System_state_counter; //Holds the system state counter
static uint8_t tmpindex; //Temp sensor decimator
//FatFS timer function
disk_timerproc();
//Incr the system uptime
Millis+=10;
if(ADC_GetFlagStatus(ADC2, ADC_FLAG_JEOC)) { //We have adc2 converted data from the injected channels
ADC_ClearFlag(ADC2, ADC_FLAG_JEOC); //Clear the flag
if(Pressure_Offset) //Only run the filter when we are sure the sensor is calibrated
Reported_Pressure=filterloop(conv_diff(ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_1)));//convert injected channel 1
//Now handle the pressure controller
if(Pressure_control&0x7F) {//If active pressure control is enabled
//run a PI controller on the air pump motor
if(Pressure_Setpoint>0 && ( Button_hold_tim>(BUTTON_TURNOFF_TIME-BUTTON_MULTIPRESS_TIMEOUT) || !Button_hold_tim ) ) {
// A Negative setpoint or prolonged button press forces a dump of air
float error=Pressure_Setpoint-Reported_Pressure;//Pressure_Setpoint is a global containing the target diff press
if(Enabled_iterations++>I_HOLDOFF) {
I+=error*PRESSURE_I_CONST;//Constants defined in main.h
if(I>PRESSURE_I_LIM) //Enforce limits
I=PRESSURE_I_LIM;
if(I<-PRESSURE_I_LIM)
I=-PRESSURE_I_LIM;
}
int16_t a=PRESSURE_P_CONST*error+I+PRESSURE_D_CONST*(Reported_Pressure-old_pressure);
if(a>0) //Make sure we are actually turning the motor on
Set_Motor((int16_t)a); //Set the motor gpio dir & pwm duty
}
else {
Enabled_iterations=0; //Make sure this is reset
if(abs(Reported_Pressure)>PRESSURE_MARGIN)
Set_Motor(-1); //Set a dump to rapidly drop to zero pressure
else
Set_Motor(0);
}
}
else if(!Pressure_control) //If the most significant bit isnt set
Set_Motor(0); //Sets the Rohm motor controller to idle (low current shutdown) state
//Check the die temperature - not possible on adc1 :-(
//Device_Temperature=convert_die_temp(ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_3));//The on die temperature sensor
#if BOARD>=3
if(Sensors&(1<<THERMISTOR_SENSOR))
Device_Temperature=convert_thermistor_temp(ADC_GetInjectedConversionValue(ADC2, ADC_InjectedChannel_3));
#endif
//Could process some more sensor data here
old_pressure=Reported_Pressure; //Set the old pressure record here for use in the D term
}
ADC_SoftwareStartInjectedConvCmd(ADC2, ENABLE); //Trigger the injected channel group
//Read any I2C bus sensors here (100Hz)
if(Sensors&(1<<TEMPERATURE_SENSOR)) {
TMP102_Reported_Temperature=GET_TMP_TEMPERATURE;
if(!tmpindex--) { //Every 30ms
tmpindex=3;
//Jobs|=1<<TMP102_CONFIG;
I2C1_Request_Job(TMP102_READ); //Request a TMP102 read if there is one present
I2C1_Request_Job(TMP102_CONFIG); //Need to do this to set one shot bit is set high again to start a new single convertion
//Some sort of i2c error here
}
}
//Now process the control button functions
if(Button_hold_tim ) { //If a button press generated timer has been triggered
if(GET_BUTTON) { //Button hold turns off the device
if(!--Button_hold_tim) {
shutdown_filesystem();
shutdown(); //Turn off the logger after closing any open files
}
}
else { //Button released - this can only ever run once per press
RED_LED_OFF; //Turn off the red LED - used to indicate button press to user
if(Button_hold_tim<BUTTON_DEBOUNCE) { //The button has to be held down for longer than the debounce period
Last_Button_Press=Millis;
if(++System_state_counter>=SYSTEM_STATES)
System_state_counter=0;//The system can only have a limited number of states
}
Button_hold_tim=0; //Reset the timer here
}
}
if(Last_Button_Press&&(Millis-Last_Button_Press>BUTTON_MULTIPRESS_TIMEOUT)&&!Button_hold_tim) {//Last press timed out and button is not pressed
if(!(System_state_Global&0x80)) //The main code has unlocked the global using the bit flag - as it has processed
System_state_Global=0x80|System_state_counter;//The previous state update
System_state_counter=0; //Reset state counter here
Last_Button_Press=0; //Reset the last button press timestamp, as the is no button press in play
}
}
void CONTROL(float rol_now, float pit_now, float yaw_now, u16 throttle, float rol_tar, float pit_tar, s16 yaw_gyro_tar)
{
//static u8 thr_yaw_flag = 0;//遥控器控制YAW标志位 1为真
//static float temp_yaw = 0;//每次用遥控器进行YAW控制时，保存的临时YAW值
float rol_error = rol_now - rol_tar;//计算角度差值
float pit_error = pit_now - pit_tar;
float yaw_error = yaw_now - (yaw_gyro_tar/12);//YAW差值（yaw_gyro_tar/12 = 0-30）
float yaw_gyro_now = number_to_dps(0/*gyro[2]*/);//YAW轴的当前角速度
//-----------------------Pitch\Roll的PID控制-START-----------------------------
PID_ROL.pout = PID_ROL.P * rol_error; //输出的P值
PID_PIT.pout = PID_PIT.P * pit_error;
//输出的I值
PID_ROL.iout += PID_ROL.I * rol_error;
PID_ROL.iout = Get_MxMi(PID_ROL.iout,PID_ROL.IMAX,-PID_ROL.IMAX);//判读I是否超出范围
PID_PIT.iout += PID_PIT.I * pit_error;
PID_PIT.iout = Get_MxMi(PID_PIT.iout,PID_PIT.IMAX,-PID_PIT.IMAX);
if(throttle<1100)//当油门小于1100时，I值清零
{
PID_ROL.iout = 0;
PID_PIT.iout = 0;
}
PID_ROL.dout = PID_ROL.D * number_to_dps(0/*gyro[0]*/);//输出的D值（与遥控器的输入无关）
PID_PIT.dout = PID_PIT.D * number_to_dps(0/*gyro[1]*/);
//-----------------------Pitch\Roll的PID控制-END-------------------------------
//-------------------YAW的PID控制-START-----------------------------
if(yaw_gyro_tar>=-5 && yaw_gyro_tar<= 5)//YAW摇杆死区控制
{
yaw_gyro_tar = 0;
}
else
{
//Q_ANGLE.YAW = 0;//YAW角度清零
//YAW角度保持不变(IMUupdate中)
}
PID_YAW.pout = PID_YAW.P * yaw_error;//P
PID_YAW.iout = 0;//I
PID_YAW.dout = PID_YAW.D * (yaw_gyro_now-yaw_gyro_tar);//D 防止YAW轴转动
//---------------------YAW的PID控制-END-----------------------------
PID_ROL.OUT = PID_ROL.pout + PID_ROL.iout + PID_ROL.dout;////PID值相加
PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;
if(ARMED == 1)
{
if(throttle>=1100)//对油门进行进行判断<1100的油门电机不转动！！！！！(为了安全很重要！！！)
{
moto1 = throttle + PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT; moto1 = Get_MxMi(moto1,2000,1000);
moto2 = throttle - PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT; moto2 = Get_MxMi(moto2,2000,1000);
moto3 = throttle - PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT; moto3 = Get_MxMi(moto3,2000,1000);
moto4 = throttle + PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT; moto4 = Get_MxMi(moto4,2000,1000);
Set_Motor(moto1,moto2,moto3,moto4);//电机PWM输出
}
else
{
moto1 =1000;moto2 =1000;moto3 =1000;moto4 =1000;
Set_Motor(moto1,moto2,moto3,moto4);//电机PWM输出
}
}
else
{
moto1 =1000;moto2 =1000;moto3 =1000;moto4 =1000;
Set_Motor(moto1,moto2,moto3,moto4);//电机PWM输出
}
}
