Submitted version of the thesis - Airlab, the Artificial Intelligence ...

114 Appendix B. Documentation of the programming
void Led_SM(void);
void Print_Int(int a, u8 * str);
void vectorByMatrix3x1(double A[][3], double x[3],
double b[3]);
void vectorByMatrix4x1(double A[][4], double x[4],
double b[4]);
void Matrix_Mult(double a1[][4], double a2[][4],
double a3[][4]);
void Matrix_Mult3334(double a1[][3], double a2[][4],
double a3[][4]);
void Matrix_Mult3443(double a1[][4], double a2[][3],
double a3[][3]);
void Inverse(double A[][3], double X[][3]);
void vectorByMatrix3x1_s16(s16 A[][3], s16 x[3], s16 b[3]);
/* Private functions ---------------------------------------------------------*/
void Led_SM(void) {
static int Count = 0;
}
Count++;
if (Count < 50000) {
LED_ON(LEDR);
} else {
LED_OFF(LEDR);
if (Count > 60000) {
Count = 0;
}
}
void Print_Int(int a, u8 * str) {
u8 myStr[10];
USB_WriteString(str);
USB_WriteString(":");
Num2String(a, myStr);
USB_WriteString(myStr);
USB_WriteString("\r\n");
}
void vectorByMatrix3x1(double A[][3], double x[3],
double b[3]) {
b[0] = A[0][0] * x[0] + A[0][1] * x[1] + A[0][2] * x[2];
b[1] = A[1][0] * x[0] + A[1][1] * x[1] + A[1][2] * x[2];
b[2] = A[2][0] * x[0] + A[2][1] * x[1] + A[2][2] * x[2];
}
void vectorByMatrix3x1_s16(s16 A[][3], s16 x[3], s16 b[3]) {
b[0] = A[0][0] * x[0] / 100 + A[0][1] * x[1] / 100
+ A[0][2] * x[2] / 100;