Atsumi Toda / Mbed 2 deprecated Auto_pilot_prototype_2

ドローン用計測制御基板の作り方vol.2で使用したピッチ制御プログラムです。

Dependencies:   mbed MPU6050_alter SDFileSystem

Vector/Vector_Matrix_operator.cpp@0:e647f6de3d26, 2020-03-06 (annotated)

Committer:
Joeatsumi
Date:
Fri Mar 06 15:03:57 2020 +0000
Revision:
0:e647f6de3d26
An software for autopilot(ver2)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:e647f6de3d26 1 #include "Vector_Matrix_operator.h"
Joeatsumi 0:e647f6de3d26 2
Joeatsumi 0:e647f6de3d26 3 Vector operator*(const Matrix& lhm, const Vector& rhv) {
Joeatsumi 0:e647f6de3d26 4 if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention");
Joeatsumi 0:e647f6de3d26 5 Vector retVec(lhm.GetRow());
Joeatsumi 0:e647f6de3d26 6
Joeatsumi 0:e647f6de3d26 7 for (int i = 1; i <= lhm.GetRow(); i++) {
Joeatsumi 0:e647f6de3d26 8 float temp = 0.0f;
Joeatsumi 0:e647f6de3d26 9 for (int j = 1; j <= rhv.GetDim(); j++) {
Joeatsumi 0:e647f6de3d26 10 temp += lhm.GetComp(i, j)*rhv.GetComp(j);
Joeatsumi 0:e647f6de3d26 11 }
Joeatsumi 0:e647f6de3d26 12 retVec.SetComp(i, temp);
Joeatsumi 0:e647f6de3d26 13 }
Joeatsumi 0:e647f6de3d26 14
Joeatsumi 0:e647f6de3d26 15 retVec.CleanUp();
Joeatsumi 0:e647f6de3d26 16
Joeatsumi 0:e647f6de3d26 17 return retVec;
Joeatsumi 0:e647f6de3d26 18 }
Joeatsumi 0:e647f6de3d26 19
Joeatsumi 0:e647f6de3d26 20 Vector operator*(const Vector& lhv, const Matrix& rhm) {
Joeatsumi 0:e647f6de3d26 21 if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention");
Joeatsumi 0:e647f6de3d26 22 Vector retVec(rhm.GetCol());
Joeatsumi 0:e647f6de3d26 23
Joeatsumi 0:e647f6de3d26 24 for (int i = 1; i <= rhm.GetCol(); i++) {
Joeatsumi 0:e647f6de3d26 25 float temp = 0.0f;
Joeatsumi 0:e647f6de3d26 26 for (int j = 1; j <= lhv.GetDim(); j++) {
Joeatsumi 0:e647f6de3d26 27 temp += lhv.GetComp(j) * rhm.GetComp(j, i);
Joeatsumi 0:e647f6de3d26 28 }
Joeatsumi 0:e647f6de3d26 29 retVec.SetComp(i, temp);
Joeatsumi 0:e647f6de3d26 30 }
Joeatsumi 0:e647f6de3d26 31
Joeatsumi 0:e647f6de3d26 32 retVec.CleanUp();
Joeatsumi 0:e647f6de3d26 33
Joeatsumi 0:e647f6de3d26 34 return retVec;
Joeatsumi 0:e647f6de3d26 35 }