Пример #1
0
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);
		}
}
Пример #2
0
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;

}
Пример #3
0
/*
 * 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);
  }
}