void datalog(void)
{
float nick, roll, yaw;
uint32_t system_time;
//update_IMU(); //Ersetzen durch Interrupt Handler!!!!!!
nick = getEuler_nick();
roll = getEuler_roll();
yaw = getEuler_yaw();
if(Datalogger_ready() && !datalog_main_opened)
{
//rc_main = f_mkfs(0,0,0);
rc_main = f_open(&Fil_Main, "QuadMain.TXT", FA_WRITE | FA_CREATE_ALWAYS);
if(rc_main != FR_OK)
{
chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: f_open() failed %d\r\n", rc_main);
return;
}
//rc_main = f_printf(&Fil, "moin\r\n");
rc_main = f_sync(&Fil_Main);
if(rc_main != FR_OK)
{
chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: f_sync() failed %d\r\n", rc_main);
return;
}
chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: opened successfull\r\n");
f_printf(&Fil_Main, "Time_Main; Nick_Main; Roll_Main; Yaw_Main\r\n");
f_sync(&Fil_Main);
datalog_main_opened = TRUE;
}
if(Datalogger_ready() && datalog_main_opened)
{
system_time = chTimeNow();
f_printf(&Fil_Main, "%d;%d;%d;%d\r\n",system_time,(int)(nick*100),(int)(roll*100),(int)(yaw*100));
rc_main = f_sync(&Fil_Main);
}
}
void Regelung(void)
{
/// Werte übernehmen
inSchub = getSchub();
inNickSollLage = 0;
inRollSollLage = getRoll();
inYawSollLage = 0;
inYawIstLage = getEuler_yaw();
inNickIstLage = getEuler_nick();
inRollIstLage = getEuler_roll();
inRollIstV = getRate_roll();
inNickIstV = getRate_nick();
inYawIstV = getRate_yaw();
/////////////////////////// Nick-Regler berechnen //////////////////////////////////////////
//äußerer Regler
ea_Nick = (inNickSollLage) - (inNickIstLage+0.08*inNickIstV); // Eingang in den äußeren Regler
if(inSchub > 0.1 && inSchub <=1 && inNickIstLage < 0.2 && inNickIstLage > -0.2)
ia_Nick = 0.01 * ea_Nick + ia_Nick; //I-Anteil nur nahe der Nulllage verändern
//Soll_v_Nick = ea_Nick*1 + ia_Nick*0.02; //Sollgeschwindigkeit des äußeren Reglers
//Soll_v_Nick = inNickSollLage*2.F;
Soll_v_Nick = 0;
//innerer Regler
v_Nick_tp1 = 0.9*v_Nick_tp1 + 0.1*inNickIstV; // Tiefpass-gefilterter Gyrowert
ei_Nick= Soll_v_Nick - v_Nick_tp1; //Eingang in die innere Regelung
pi_Nick = ei_Nick * 0.07; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 /*&& inNickIstLage < 0.5 && inNickIstLage > -0.5*/)
ii_Nick = 0.08 * ei_Nick + ii_Nick; //I-Anteil nur nahe der Nulllage verändern
di_Nick = (ei_Nick - ei_Nick_alt)*10; //d-Anteil
if (di_Nick > 1.5)
di_Nick = 1.5;
else if(di_Nick < -1.5)
di_Nick = -1.5; //Saturierung des D-Anteils
aNick = (pi_Nick + (ii_Nick)*0.03 + di_Nick)*567; //Ausgang des inneren Reglers
/////////////////////////// Roll-Regler berechnen //////////////////////////////////////////
/*ea_Roll = (inRollSollLage) - (inRollIstLage+0.08*inRollIstV); // Eingang in den äußeren Regler
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ia_Roll = 0.01 * ea_Roll + ia_Roll; //I-Anteil nur nahe der Nulllage verändern
Soll_v_Roll = ea_Roll*2 + ia_Roll*0.02; //Sollgeschwindigkeit des äußeren Reglers
//Soll_v_Roll = inRollSollLage;
//innerer Regler
v_Roll_tp1 = 0.95*v_Roll_tp1 + 0.05*inRollIstV; // Tiefpass-gefilterter Gyrowert
ei_Roll= Soll_v_Roll - v_Roll_tp1; //Eingang in die innere Regelung
pi_Roll = ei_Roll * 0.5; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ii_Roll = 0.1 * ei_Roll + ii_Roll; //I-Anteil nur nahe der Nulllage verändern
di_Roll = (ei_Roll - ei_Roll_alt)*20; //d-Anteil
if (di_Roll > 1.5)
di_Roll = 1.5;
else if(di_Roll < -1.5)
di_Roll = -1.5; //Saturierung des D-Anteils
aRoll = (pi_Roll + (ii_Roll+37)*0.02 + di_Roll)*567; //Ausgang des inneren Reglers */
ea_Roll = (inRollSollLage) - (inRollIstLage+0.08*inRollIstV); // Eingang in den äußeren Regler
if(inSchub > 0.1 && inSchub <=1 && inRollIstLage < 0.2 && inRollIstLage > -0.2)
ia_Roll = 0.01 * ea_Roll + ia_Roll; //I-Anteil nur nahe der Nulllage verändern
Soll_v_Roll = ea_Roll*2 + ia_Roll*0.02; //Sollgeschwindigkeit des äußeren Reglers
Soll_v_Roll = inRollSollLage;
//innerer Regler
v_Roll_tp1 = 0.95*v_Roll_tp1 + 0.05*inRollIstV; // Tiefpass-gefilterter Gyrowert
ei_Roll= Soll_v_Roll - v_Roll_tp1; //Eingang in die innere Regelung
pi_Roll = ei_Roll * 0.1; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 /*&& inRollIstLage < 0.2 && inRollIstLage > -0.2*/)
ii_Roll = 0.01 * ei_Roll + ii_Roll; //I-Anteil nur nahe der Nulllage verändern
di_Roll = (ei_Roll - ei_Roll_alt)*10; //d-Anteil
if (di_Roll > 1.5)
di_Roll = 1.5;
else if(di_Roll < -1.5)
di_Roll = -1.5; //Saturierung des D-Anteils
aRoll = (pi_Roll + (ii_Roll)*0.005 + di_Roll)*567; //Ausgang des inneren Reglers
/////////////////////////// Yaw-Regler berechnen //////////////////////////////////////////
ea_Yaw = (inYawSollLage) - (inYawIstLage+0.08*inYawIstV); // Eingang in den äußeren Regler
if(inSchub > 0.1 && inSchub <=1 && inYawIstLage < 0.2 && inYawIstLage > -0.2)
ia_Yaw = 0.01 * ea_Yaw + ia_Yaw; //I-Anteil nur nahe der Nulllage verändern
Soll_v_Yaw = ea_Yaw*0.5 + ia_Yaw*0.002; //Sollgeschwindigkeit des äußeren Reglers
//Soll_v_Yaw = inYawSollLage;
//innerer Regler
v_Yaw_tp1 = 0.99*v_Yaw_tp1 + 0.01*inYawIstV; // Tiefpass-gefilterter Gyrowert
ei_Yaw= Soll_v_Yaw - v_Yaw_tp1; //Eingang in die innere Regelung
pi_Yaw = ei_Yaw * 0.4; //p-Anteil
if(inSchub > 0.1 && inSchub <=1 && inYawIstLage < 0.2 && inYawIstLage > -0.2)
ii_Yaw = 0.01 * ei_Yaw + ii_Yaw; //I-Anteil nur nahe der Nulllage verändern
di_Yaw = (ei_Yaw - ei_Yaw_alt)*20; //d-Anteil
if (di_Yaw > 1.5)
di_Yaw = 1.5;
else if(di_Yaw < -1.5)
di_Yaw = -1.5; //Saturierung des D-Anteils
aYaw = (pi_Yaw + (ii_Yaw)*0.1 + di_Yaw)*567; //Ausgang des inneren Reglers
if (aYaw > 2000.F)
aYaw = 2000.F;
else if(aYaw < -2000.F)
aYaw = -2000.F; //Saturierung des Yaw-Anteils
/////////////////////////// Motorwerte setzen //////////////////////////////////////////
if(inSchub > 0.1 && inSchub <=1)
{
Schub_Offset = 6000 * inSchub;
aNick = 0.F;
aYaw = 0.F;
outMotor3 = Schub_Offset - aRoll + aYaw;
//outMotor1 = Schub_Offset + aNick - aYaw;
//outMotor4 = Schub_Offset - aNick - aYaw;
outMotor1 = 0.F;
outMotor4 = 0.F;
outMotor2 = Schub_Offset + aRoll + aYaw;
}
else
{
outMotor1 = 0.F;
outMotor2 = 0.F;
outMotor3 = 0.F;
outMotor4 = 0.F;
}
/////////////////////////// Motorwerte saturieren und übergeben ////////////////////////
if (outMotor1 > 6800.F) setMotor_1(6800.F);
else if(outMotor1 < 0.F) setMotor_1(0.F);
else setMotor_1(outMotor1);
if (outMotor2 > 6800.F) setMotor_2(6800.F);
else if(outMotor2 < 0.F) setMotor_2(0.F);
else setMotor_2(outMotor2);
if (outMotor3 > 6800.F) setMotor_3(6800.F);
else if(outMotor3 < 0.F) setMotor_3(0.F);
else setMotor_3(outMotor3);
if (outMotor4 > 6800.F) setMotor_4(6800.F);
else if(outMotor4 < 0.F) setMotor_4(0.F);
else setMotor_4(outMotor4);
/////////////////////////// Alte Werte merken //////////////////////////////////////////
v_Nick_tp1_alt = v_Nick_tp1;
ei_Nick_alt = ei_Nick;
v_Roll_tp1_alt = v_Roll_tp1;
ei_Roll_alt = ei_Roll;
v_Yaw_tp1_alt = v_Yaw_tp1;
ei_Yaw_alt = ei_Yaw;
}
/*
* Application entry point.
*/
int main(void)
{
float nick, roll, yaw;
static const evhandler_t evhndl[] = {InsertHandler, RemoveHandler};
struct EventListener el0, el1;
FRESULT err;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 2 using the driver default configuration.
* PA2(TX) and PA3(RX) are routed to USART2.
*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOD, 5, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOD, 6, PAL_MODE_ALTERNATE(7));
setup_IMU();
setup_Fernsteuerung();
setup_Motoren();
setup_Regelung();
// initialize MMC driver
// setup pads to SPI1 function (connect these pads to your SD card accordingly)
palSetPadMode(GPIOC, 4, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); // NSS
palSetPadMode(GPIOA, 5, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // SCK
palSetPadMode(GPIOA, 6, PAL_MODE_ALTERNATE(5)); // MISO
palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_HIGHEST); // MOSI
palSetPad(GPIOC, 4); // set NSS high
// initialize MMC driver
mmcObjectInit(&MMCD1, &SPID1, &ls_spicfg, &hs_spicfg, mmc_is_protected, mmc_is_inserted);
mmcStart(&MMCD1, NULL);
chEvtRegister(&MMCD1.inserted_event, &el0, 0);
chEvtRegister(&MMCD1.removed_event, &el1, 1);
chThdSleepMilliseconds(7000);
if(mmcConnect(&MMCD1))
{
chprintf((BaseChannel *) &SD2, "SD: Failed to connect to card\r\n");
return;
}
else
{
chprintf((BaseChannel *) &SD2, "SD: Connected to card\r\n");
}
err = f_mount(0, &MMC_FS);
if(err != FR_OK)
{
chprintf((BaseChannel *) &SD2, "SD: f_mount() failed %d\r\n", err);
mmcDisconnect(&MMCD1);
return;
}
else
{
chprintf((BaseChannel *) &SD2, "SD: File system mounted\r\n");
}
fs_ready = TRUE;
rc = f_open(&Fil, "Quad.TXT", FA_WRITE | FA_CREATE_ALWAYS);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state, when the button is
* pressed the test procedure is launched with output on the serial
* driver 2.
*/
while (TRUE)
{
update_IMU(); //Ersetzen durch Interrupt Handler!!!!!!
nick = getEuler_nick();
roll = getEuler_roll();
yaw = getEuler_yaw();
f_printf(&Fil, "%d;%d;%d\r\n",(int)(nick*100),(int)(roll*100),(int)(yaw*100));
rc = f_sync(&Fil);
chThdSleepMilliseconds(10);
}
}
