Korkens styrkort för multirotors (KFly) - Nya bilder s. 27!

[Korken]

Universitetet gjorde en liten artikel om mig, kan vara lite trevlig läsning.
http://www.ltu.se/ltu/media/Student-gor ... t-1.105219

[PeterH]

Jättekul! Ett stort grattis till ett utmärkt och spännande arbete

Jag förstår att du har fullt upp men frågar ändå om framtidsplaner för att släppa en version av K-fly för alla oss som fortfarande drömmer om en quad...

[lizerdboy]

Fasen vad kul, hur grymt som helst

[malx]

Angående om en allmän tillgänglig K-Fly får Emil svara på .

Vad jag kan säga är att jag har ett av hans kort och jobbar på en portning av OpenPilot/Taulabs till kortet. Lite för tidigt för att säga vilka delar som kommer fungera och vilka som kommer fungera sämre men jag är relativt säker på att inom några veckor så flyger kortet iallafall. Emil jobbar parallellt med sin egen kod men jag vet att han har rätt mycket att göra så jag tror det dröjer innan den koden är flygbar.

(K-fly har samma sensorer o.s.v. som de flesta av de nya "OP"-korten, dock använder den sig av andra in- och utgångar på processorn vilket kan ge ett annorlunda spann på vad som den klarar och inte klarar)

Vad jag kommer att göra när jag har lyckats med portningen är att testa Emils regulator i Taulabs-koden, på det sättet så kan man få en direkt jämförelse genom att bara switcha mellan regulatorerna.

Som bonus skickar jag med en första snabbskiss på miniquaden jag kommer testa kortet på:
Utskriven på 3D-skrivare med gummibandfastsatta armar. Förutom den förenklade transporten så ska armarna ska lossna vid ev. krasch för att spara på de veka motoraxlarna som annars böjer sig och tyvärr ej går att ersätta på ett bra sätt.

SilverHornet2.jpg

[Korken]

Tackar!

PeterH:
Som malx påpekar så har jag tyvärr väldigt mycket just nu, jag är på slutspurten på mitt exjobb och det äter upp min existens just nu.
Men det som är det största problemet är om folk ska få tillgång till kortet är att det tar mycket tid att tillverka varje kort, helst skulle jag vilja få de monterade.
Jag har kollat upp detta och det kostar ca 200kr/kort om jag köper 100 kort men då måste jag också köpa just 100 kort.
Men vill ett fåtal ha kort så kan jag montera ett gäng mot sommaren.

malx:
Jag ser att ramen börjar komma till liv.
Hur skulle du göra för att få armarna avtagbara?

[Agwan]

Jag förstår inte riktigt vad som är det speciella i det du gjort efter att ha läst artikeln. Är det att använda kvarternioner, eller använder du kvarternioner på ett nytt annorlunda sätt?

[Korken]

Hela artikeln finns inte ute än, men vad jag har gjort är att jag använder kvarternioner på ett nytt sätt så man inte behöver göra några omräkningar alls mellan grader (Euler vinklar) eller använda sig an en DCMs (Direction Cosine Matrix) kopplade differentialekvationer i reglerloopen.
Dvs den blir extremt beräkningseffektiv och har inga singulariteter.

[NULL]

Förstår inte matematiken så klart, men på LTU-artikeln låter det som att du har löst JAS-krash-problemet.

[Korken]

Hehe, det kanske jag har!

[malx]

Med tanke på senaste tidningsartiklarna så verkar försvaret redan ha löst problemet galant genom att aldrig lyfta.

Så effektiv kod kommer du aldrig att skriva så du tar mindre datakraft än den lösningen Korken!

[superx]

Grattis! Jag tycker det är väldigt kul när lite mer avancerad matte kommer till användning. Kommer det gå att läsa artikeln någonstans?

[Korken]

malx:
Haha, det är sant! Men tror ryssen kommer och tar chans på det

superx:
Tackar! Jodå, men inte föräns i Juli när konferensen har varit. Tills dess är den tyvärr konfidentiell.

[Agwan]

Jag har ett kort jag använder för att mäta vinkel mot jordens masscentrum i två led. På det kortet sitter en accelerometer och ett gyro, vardera tre axlar. Idag kör jag ett kalmanfilter på kortets 8-bitars AVR, som går på 8 MHz, för att väga samman mätningarna. Men jag kör i euler-vinklar och får såklart problem i vissa vinklar.

Kommer jag kunna använda din metod för att fixa vinkelproblematiken och kanske förbättra prestanda? Hur komplicerat är det att använda din lösning?

[Korken]

Det kan jag nästan garantera!
Jag har redan ett dokument för att göra estimering med kvarternioner (det du gör, fast med kvarternioner) men de tar också hänsyn till magnetfältet.
Du kan säkerligen använda detta, men du måste nog kollapsa lite av ekvationerna för att bli av med magnetmätningarna.
Dock så måste du nog köra med fixed point samt inte allt för hög uppdateringshastighet, detta är vart AVRen kommer sätta stopp.

Dokument om estimering:

kalman-quaternion-AHRS.pdf

Vill du dock göra det väldigt enkelt, kör med Madgwicks estimerings algoritm.
Den kör med ett komplementärfilter istället för Kalman filter (dvs den är mycket snabbare att exekvera) och det fungerar mycket bra!
Jag slänger in koden här också:

MadgwickAHRS.h

Kod: Markera allt

//=====================================================================================================
// MadgwickAHRS.h
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimized for reduced CPU load
//
//=====================================================================================================
#ifndef MadgwickAHRS_h
#define MadgwickAHRS_h

//----------------------------------------------------------------------------------------------------
// Variable declaration

extern volatile float beta;				// algorithm gain
extern volatile float q0, q1, q2, q3;	// quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);

#endif
//=====================================================================================================
// End of file
//=====================================================================================================

MadgwickAHRS.c

Kod: Markera allt

//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimized for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalized
// 07/02/2013	Emil Fresk		Fixed error in math, wrong derivative direction and wrong frame
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include "MadgwickAHRS.h"
#include <math.h>

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq	200.0f		// sample frequency in Hz
#define betaDef		0.1f		// 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;								// 2 * proportional gain (Kp)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float hx, hy;
	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalization)
	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
		MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
		return;
	}

	// Rate of change of quaternion from gyroscope
//	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
//	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
//	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
//	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
	qDot1 = -0.5f * (- q1 * gx - q2 * gy - q3 * gz);
	qDot2 = -0.5f * (q0 * gx + q3 * gy - q2 * gz);
	qDot3 = -0.5f * (- q3 * gx + q0 * gy + q1 * gz);
	qDot4 = -0.5f * (q2 * gx - q1 * gy + q0 * gz);

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalization)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Normalise magnetometer measurement
		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
		mx *= recipNorm;
		my *= recipNorm;
		mz *= recipNorm;

		// Auxiliary variables to avoid repeated arithmetic
		_2q0mx = 2.0f * q0 * mx;
		_2q0my = 2.0f * q0 * my;
		_2q0mz = 2.0f * q0 * mz;
		_2q1mx = 2.0f * q1 * mx;
		_2q0 = 2.0f * q0;
		_2q1 = 2.0f * q1;
		_2q2 = 2.0f * q2;
		_2q3 = 2.0f * q3;
		_2q0q2 = 2.0f * q0 * q2;
		_2q2q3 = 2.0f * q2 * q3;
		q0q0 = q0 * q0;
		q0q1 = q0 * q1;
		q0q2 = q0 * q2;
		q0q3 = q0 * q3;
		q1q1 = q1 * q1;
		q1q2 = q1 * q2;
		q1q3 = q1 * q3;
		q2q2 = q2 * q2;
		q2q3 = q2 * q3;
		q3q3 = q3 * q3;

		// Reference direction of Earth's magnetic field
		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
		_2bx = sqrt(hx * hx + hy * hy);
		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
		_4bx = 2.0f * _2bx;
		_4bz = 2.0f * _2bz;

		// Gradient decent algorithm corrective step
		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // Normalize step magnitude
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		// Apply feedback step
		qDot1 -= beta * s0;
		qDot2 -= beta * s1;
		qDot3 -= beta * s2;
		qDot4 -= beta * s3;
	}

	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

	// Normalize quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

//---------------------------------------------------------------------------------------------------
// IMU algorithm update

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float s0, s1, s2, s3;
	float qDot1, qDot2, qDot3, qDot4;
	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

	// Rate of change of quaternion from gyroscope
	//qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
	//qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
	//qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
	//qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
	qDot1 = -0.5f * (- q1 * gx - q2 * gy - q3 * gz);
	qDot2 = -0.5f * (q0 * gx + q3 * gy - q2 * gz);
	qDot3 = -0.5f * (- q3 * gx + q0 * gy + q1 * gz);
	qDot4 = -0.5f * (q2 * gx - q1 * gy + q0 * gz);

	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Auxiliary variables to avoid repeated arithmetic
		_2q0 = 2.0f * q0;
		_2q1 = 2.0f * q1;
		_2q2 = 2.0f * q2;
		_2q3 = 2.0f * q3;
		_4q0 = 4.0f * q0;
		_4q1 = 4.0f * q1;
		_4q2 = 4.0f * q2;
		_8q1 = 8.0f * q1;
		_8q2 = 8.0f * q2;
		q0q0 = q0 * q0;
		q1q1 = q1 * q1;
		q2q2 = q2 * q2;
		q3q3 = q3 * q3;

		// Gradient decent algorithm corrective step
		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
		s0 *= recipNorm;
		s1 *= recipNorm;
		s2 *= recipNorm;
		s3 *= recipNorm;

		// Apply feedback step
		qDot1 -= beta * s0;
		qDot2 -= beta * s1;
		qDot3 -= beta * s2;
		qDot4 -= beta * s3;
	}

	// Integrate rate of change of quaternion to yield quaternion
	q0 += qDot1 * (1.0f / sampleFreq);
	q1 += qDot2 * (1.0f / sampleFreq);
	q2 += qDot3 * (1.0f / sampleFreq);
	q3 += qDot4 * (1.0f / sampleFreq);

	// Normalise quaternion
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

[Walle]

Det är sånt här som får mig att verkligen ångra att matematik inte intresserade mig alls i gymnasiet. Nu över tio år efter studenten intresserar matematiken mig, men jag förstår absolut noll av vad det är du gör (på det matematiska planet förstås). Är det ens möjligt att beskriva det kortfattat i termer som någon med enbart gymnasiets matte A iaf kan förstå vad det handlar om även om man inte förstår exakt hur det går till?

Stort grattis till stipendiet och den kommande publiceringen, det måste kännas riktigt kul!