Team_temp
/
VT2_domc_copy
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed-dev
Fork of
Adafruit9-DOf_AHRS_Regler_Discrete
by 
RT-Labor IMS
Revision 3:cc828af4a4c6, committed 2018-09-20
- Comitter:
- rtlabor
- Date:
- Thu Sep 20 07:43:09 2018 +0000
- Parent:
- 2:9fd59c70ad78
- Commit message:
- Madgwick Filter mit Quaternionen Ausgabe, Achtung, teilweise muss neu verbunden werden
Changed in this revision
--- a/Header/MadgwickAHRS.h Thu Oct 19 11:52:10 2017 +0000
+++ b/Header/MadgwickAHRS.h Thu Sep 20 07:43:09 2018 +0000
@@ -70,5 +70,6 @@
if (!anglesComputed) computeAngles();
return yaw;
}
+ float Q[4];
};
#endif
--- a/Source/Adafruit_L3GD20_U.cpp Thu Oct 19 11:52:10 2017 +0000
+++ b/Source/Adafruit_L3GD20_U.cpp Thu Sep 20 07:43:09 2018 +0000
@@ -80,7 +80,7 @@
for correct address and that the IC is properly connected */
uint8_t id = 0;
id = read8(GYRO_REGISTER_WHO_AM_I);
- Serial serial( USBTX, USBRX);
+ Serial serial( USBTX, USBRX, 115200);
serial.printf("WHOAMI Gyro %d \r\n", id);
if ((id != L3GD20_ID) && (id != L3GD20H_ID))
{
--- a/Source/MadgwickAHRS.cpp Thu Oct 19 11:52:10 2017 +0000
+++ b/Source/MadgwickAHRS.cpp Thu Sep 20 07:43:09 2018 +0000
@@ -254,4 +254,5 @@
pitch = asinf(-2.0f * (q1*q3 - q0*q2));
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
anglesComputed = 1;
+ Q[0]=q0;Q[1]=q1;Q[2]=q2;Q[3]=q3;
}
\ No newline at end of file
--- a/Source/main.cpp Thu Oct 19 11:52:10 2017 +0000
+++ b/Source/main.cpp Thu Sep 20 07:43:09 2018 +0000
@@ -9,28 +9,6 @@
*/
#define PI 3.1415926536
-/* Choose PID-T1 parameters VELOCITY Controller */
-//P-Part
-#define P_gain 1.666666666666668//2.5
-#define P_b01 1.0
-#define P_b11 -0.843146028951280
-#define P_a11 -0.738576714918798
-//I-Part
-#define I_gain 0.050505050505053
-#define I_b11 1.0
-#define I_a11 -1.0
-//Pre-Gain
-#define k 0.053348382301167*2
-
-/* Choose P parameter POSITION Controller */
-#define KpPosition 25*1.5
-
-/* Choose setposition for the gimbal */
-#define SOLLPOSITION 0 // 30° = -0.523598775598299 //[rad]
-
-
-/* Assign a unique ID to the sensors */
-//This board/chip uses I2C 7-bit addresses 0x19 & 0x1E & 0x6B
Adafruit_9DOF dof = Adafruit_9DOF();
@@ -41,8 +19,7 @@
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(30303);
//Debug/Communication with PC through
-Serial pc(PA_0, PA_1, 115200); //921600; 115200
-Serial serial1(USBTX, USBRX, 9600);
+Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer
// Magnetometer correction
// mounted in gimbal
@@ -84,7 +61,7 @@
//Timer for controller interrupt
// the controll loop will be atached to the timer interrupt
Ticker ControllerLoopTimer;
-void updateControllers(void);
+void updateAHRS(void);
//Sensor events, contains the last read sensor values
sensors_event_t mag_event;
@@ -120,13 +97,13 @@
void initSensors()
{
-
+ pc.printf(("Init...\r\n"));
if(!accel.begin())
{
/* There was a problem detecting the LSM303 ... check your connections */
- pc.printf(("Ooops, no LSM303 detected ... Check your wiring!"));
+ pc.printf(("Ooops, no LSM303 detected ... Check your wiring!\r\n"));
while(1);
@@ -136,7 +113,7 @@
/* There was a problem detecting the LSM303 ... check your connections */
- pc.printf("Ooops, no LSM303 detected ... Check your wiring!");
+ pc.printf("Ooops, no LSM303 detected ... Check your wiring!\r\n");
while(1);
@@ -149,7 +126,7 @@
/* There was a problem detecting the LSM303 ... check your connections */
- pc.printf("Ooops, no L3GD20 detected ... Check your wiring!");
+ pc.printf("Ooops, no L3GD20 detected ... Check your wiring!\r\n");
while(1);
@@ -178,29 +155,35 @@
{
//init system
setup();
+
//set PWM frequncy to 1 kHZ
- SollStrom.period(0.001);
- //set Current to 0 Ampère
- SollStrom.write(0.5);
- //enable motor
- motorEnable.write(1);
-
- //wait for start (user button press)
- while( SystemEnable )
- {
- serial1.printf("Wait for system enable! \r\n");
- }
- while( !SystemEnable ){}
- //attach controller loop to timer interrupt
- ControllerLoopTimer.attach(&updateControllers, 0.003030303030303f); //Assume Ts = 1/330;
+ wait_ms(500);
+ pc.printf("Started! \r\n");
+ //ControllerLoopTimer.attach(&updateControllers, 1.0f); //Assume Ts = 1/100;
//controller on -> LED on
- SystemEnableLED = 1;
- //start timer for sample time estimation
t.start();
//do as long user button d'ont get pressed
- while ( SystemEnable )
+ while ( 1 )// schafft etwa 400Hz
{
- //read raw sensor data
+ LoopCounter++;
+ updateAHRS();
+ if(LoopCounter == 100)
+ {
+ LoopCounter = 0;
+ //pc.printf("R %3.2f P %3.2f Y %3.2f \r\n",filter.getRoll(),filter.getPitch(),filter.getYaw());
+ //pc.printf("R %2.4f P %2.4f Y %2.4f \r\n",gyro_event.gyro.x,gyro_event.gyro.y,gyro_event.gyro.z);
+ filter.getRoll();
+ pc.printf("A %f B %f C %f D %f\r\n",filter.Q[0],filter.Q[1],filter.Q[2],filter.Q[3]);
+ }
+ //wait_ms(100);
+
+ }
+}
+
+
+void updateAHRS(void)
+{
+
mag.getEvent(&mag_event);
accel.getEvent(&accel_event);
gyro.getEvent(&gyro_event);
@@ -233,121 +216,6 @@
//Perform Madgwick-filter update
filter.update( CorrectedGyrometer[0]/ PI * 180, -CorrectedGyrometer[1]/ PI * 180, -CorrectedGyrometer[2]/ PI * 180 ,
-CorrectedAccelerometer[0] , CorrectedAccelerometer[1] , CorrectedAccelerometer[2] ,
- CorrectedMagnetometer[0] , -CorrectedMagnetometer[1], -CorrectedMagnetometer[2]);
-
- LoopCounter++;
- if(LoopCounter == 12000)
- {
- eMaxPos = 0;
- eMaxNeg = 0;
- serial1.printf("Error reseted");
- }
- //serial1.printf("Roll: %f Grad \r\n",filter.getRoll());
-
- }
- //Stop timer
- TotalTime = t.read();
- //disable motor
- motorEnable.write(0);
- //set Current to 0 Ampère
- SollStrom.write(0.5);
- //Switch controller off
- ControllerLoopTimer.detach();
- //controller off -> LED off
- SystemEnableLED = 0;
- //Print mean loop time ->sample frequency of the madgwick-filter
- serial1.printf("Mean loop time: %f [s] \r\n", TotalTime/LoopCounter);
- serial1.printf("Max pos Fehler: %f [°] \r\n", eMaxPos/PI*180);
- serial1.printf("Max neg Fehler: %f [°] \r\n", eMaxNeg/PI*180);
- serial1.printf("Finished!");
-}
-
-
-
-/**************************************************************************
-/
-/
- @brief Regler (Geschwindigkeits- und Positionsregler)
-
-/
-**************************************************************************/
-//-----------------------------------------------------------------------//
-// //
-// //
-// controller (PID-T1) //
-//ThetaDes[rad] ____ ____/ //
-// ------>O--->| Kx |----->O---->| Cv |---> I_des [A] //
-// ^- ---- ^- ---- //
-// | Pos.Cntrl. | Speed Cntrl. //
-// | | //
-// | | //
-// | +---- Speed Omega [rad/s] //
-// | //
-// +--------------------- Position Theta [rad] //
-// //
-//-----------------------------------------------------------------------//
-
-// Cv is split into a prop. and an integrating part (limiting, windup)
-// e ___ ____
-// ---->| k |---+------>| Cp |--------->O---->
-// --- | ---- ^
-// | ____ ________ |
-// +->| k_I|->| z/(z-1)|---+
-// ---- -------- \
-// Integrator
-//
-// the anti-windup loop is not shon in the diagram above! (see documentation
-// for detail)
-void updateControllers(void)
-{
- static float ek_1 = 0;
- static float ukP_1 = 0;
- static float ukI_1 = 0;
-
- float ePosition = SOLLPOSITION - filter.getRollRadians();
-
- //Positionsregler
- float stellgroessePosition = KpPosition*(ePosition);
- if( ePosition > eMaxPos)
- {
- eMaxPos = ePosition;
- }
- if( ePosition < eMaxNeg )
- {
- eMaxNeg = ePosition;
- }
-
- //Geschwindigkeitsregler
- float ek = k*(stellgroessePosition - CorrectedGyrometer[0]);
- //P-part
- float ukP = P_gain*(P_b01*ek + P_b11*ek_1) - P_a11*ukP_1;
- //I-Part
- float stellgroesseStrom = ukP + ukI_1;
- float ukI = I_gain*I_b11*ek_1 - I_a11*ukI_1;
-
- //Limit +/- 0.5 Ampère
- float uSaturated;
- if(stellgroesseStrom > 0.5)
- {
- SollStrom.write( 0.8999999 );
- uSaturated = 0.5;
- }
- else if( stellgroesseStrom < -0.5 )
- {
- SollStrom.write( 0.1000001 );
- uSaturated = -0.5;
- }
- else
- {
- SollStrom.write(0.5f+stellgroesseStrom*0.4f/0.5f);
- uSaturated = stellgroesseStrom;
- }
-
- //Anti-Windup (back calculation)
- float deltaI = ( -stellgroesseStrom + uSaturated );
- ukI_1 = ukI + deltaI*I_gain;
- ek_1 = ek;
- ukP_1 = ukP;
-
-
+ CorrectedMagnetometer[0] , -CorrectedMagnetometer[1], -CorrectedMagnetometer[2]);
+
}
\ No newline at end of file