HAPSRG / Mbed 2 deprecated HAPStail

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 MatrixMath LPS25HB_I2C LSM9DS1 Matrix2 PIDcontroller LoopTicker SBUS_without_mainfile UsaPack solaESKF_wind Vector3 CalibrateMagneto FastPWM

datatransfer.cpp@127:839ae718713b, 2022-07-08 (annotated)

Committer:
NaotoMorita
Date:
Fri Jul 08 08:25:42 2022 +0000
Revision:
127:839ae718713b
Parent:
124:59efdd87c211 
new

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 73:be7a8b8188de 1 #include "global.hpp"
NaotoMorita 73:be7a8b8188de 2
NaotoMorita 73:be7a8b8188de 3 void send2center()
NaotoMorita 73:be7a8b8188de 4 {
NaotoMorita 73:be7a8b8188de 5 valuePack& vp = posValues;
NaotoMorita 73:be7a8b8188de 6 vp.acc[0] = acc.x;
NaotoMorita 73:be7a8b8188de 7 vp.acc[1] = acc.y;
NaotoMorita 73:be7a8b8188de 8 vp.acc[2] = acc.z;
NaotoMorita 73:be7a8b8188de 9 vp.gyro[0] = gyro.x;
NaotoMorita 73:be7a8b8188de 10 vp.gyro[1] = gyro.y;
NaotoMorita 73:be7a8b8188de 11 vp.gyro[2] = gyro.z;
NaotoMorita 73:be7a8b8188de 12 vp.mag[0] = mag.x;
NaotoMorita 73:be7a8b8188de 13 vp.mag[1] = mag.y;
NaotoMorita 73:be7a8b8188de 14 vp.mag[2] = mag.z;
NaotoMorita 73:be7a8b8188de 15 vp.rpy[0] = rpy.x;
osaka 86:6b8e797306b6 16 vp.rpy[1] = rpy.y;
NaotoMorita 73:be7a8b8188de 17 vp.rpy[2] = rpy.z;
osaka 88:0fc5df2fddcb 18 vp.altitude = -palt;
NaotoMorita 73:be7a8b8188de 19 vp.de = de;
NaotoMorita 73:be7a8b8188de 20 vp.deobj = deobj;
osaka 96:0be0d80d5299 21 Matrix pihat = eskf.getPihat();
osaka 96:0be0d80d5299 22 vp.pi[0] = pihat(1, 1);
osaka 96:0be0d80d5299 23 vp.pi[1] = pihat(2, 1);
osaka 96:0be0d80d5299 24 vp.pi[2] = pihat(3, 1);
NaotoMorita 118:97ffe77b6f38 25 //pc.printf("pihat: %f %f %f\r\n", pihat(1, 1), pihat(2, 1), pihat(3, 1));
osaka 96:0be0d80d5299 26 Matrix vihat = eskf.getVihat();
osaka 96:0be0d80d5299 27 vp.vi[0] = vihat(1, 1);
osaka 96:0be0d80d5299 28 vp.vi[1] = vihat(2, 1);
osaka 96:0be0d80d5299 29 vp.vi[2] = vihat(3, 1);
osaka 91:393b9ae62681 30 if (prefligt_finished)
osaka 91:393b9ae62681 31 {
osaka 91:393b9ae62681 32 vp.readyFlag = 1;
osaka 91:393b9ae62681 33 }
osaka 91:393b9ae62681 34 else
osaka 91:393b9ae62681 35 {
osaka 91:393b9ae62681 36 vp.readyFlag = 0;
osaka 91:393b9ae62681 37 }
NaotoMorita 90:0b1f62f7a83a 38 if(updateValues.calibrationFlag == 1111){
NaotoMorita 90:0b1f62f7a83a 39 vp.calibEndFlag = 2;
NaotoMorita 90:0b1f62f7a83a 40 }else{
NaotoMorita 90:0b1f62f7a83a 41 vp.calibEndFlag = 0;
NaotoMorita 90:0b1f62f7a83a 42 }
NaotoMorita 73:be7a8b8188de 43 for(int i = 0 ; i<5 ; i++){
NaotoMorita 73:be7a8b8188de 44 vp.rc[i] = rc[i];
NaotoMorita 73:be7a8b8188de 45 }
osaka 99:37cfd8633c9b 46 vp.att_dt = att_dt;
osaka 114:ba221936d53a 47 if (rc[8] < -0.3f)
osaka 112:5a82800e1485 48 {
osaka 112:5a82800e1485 49 vp.ap_flag = 1;
osaka 112:5a82800e1485 50 }
osaka 112:5a82800e1485 51 else
osaka 112:5a82800e1485 52 {
osaka 112:5a82800e1485 53 vp.ap_flag = 0;
osaka 112:5a82800e1485 54 }
osaka 124:59efdd87c211 55 getElectricVal();
osaka 124:59efdd87c211 56 //pc.printf("%f %f\r\n", voltage, current);
osaka 123:0a74e696f7a6 57 vp.voltage = voltage;
osaka 123:0a74e696f7a6 58 vp.current = current;
osaka 99:37cfd8633c9b 59
NaotoMorita 73:be7a8b8188de 60 tail.Send(tail_address[pos_tail], &(posValues));
NaotoMorita 108:e582f8bd4729 61 // pc.printf("%f %f %f %d %d %f %f %f \r\n",rpy.x/M_PI*180.0f,rpy.y/M_PI*180.0f,rpy.z/M_PI*180.0f,vp.readyFlag,updateValues.calibrationFlag,updateValues.vx_opt,updateValues.vy_opt,updateValues.dist_ir);
NaotoMorita 73:be7a8b8188de 62 }
NaotoMorita 73:be7a8b8188de 63
NaotoMorita 118:97ffe77b6f38 64 void send2pc()
NaotoMorita 118:97ffe77b6f38 65 {
NaotoMorita 118:97ffe77b6f38 66 sp.de = de;
NaotoMorita 118:97ffe77b6f38 67 sp.dTl = dTl;
NaotoMorita 118:97ffe77b6f38 68 sp.dTr = dTr;
NaotoMorita 118:97ffe77b6f38 69 sp.dr = dr;
NaotoMorita 127:839ae718713b 70 sp.roll = rpy.x/M_PI*180.0f;
NaotoMorita 127:839ae718713b 71 sp.pitch = rpy.y/M_PI*180.0f;
NaotoMorita 127:839ae718713b 72 sp.yaw = rpy.z/M_PI*180.0f;
NaotoMorita 121:1d5b3e1f0d21 73 //sp.data[0] = vi.x;
NaotoMorita 121:1d5b3e1f0d21 74 //sp.data[1] = vi.y;
NaotoMorita 121:1d5b3e1f0d21 75 //sp.data[2] = vi.z;
NaotoMorita 118:97ffe77b6f38 76 pc.Send(0000, &(sp));
NaotoMorita 118:97ffe77b6f38 77
NaotoMorita 118:97ffe77b6f38 78 }
NaotoMorita 118:97ffe77b6f38 79
NaotoMorita 73:be7a8b8188de 80 // eeprom書き込み・読み込みに必要な関数
NaotoMorita 73:be7a8b8188de 81 void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 82 {
NaotoMorita 73:be7a8b8188de 83 char i2cBuffer[size + 2];
NaotoMorita 73:be7a8b8188de 84 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 85 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 86
NaotoMorita 73:be7a8b8188de 87 for (int i = 0; i < size; i++) {
NaotoMorita 73:be7a8b8188de 88 i2cBuffer[i + 2] = data[i];
NaotoMorita 73:be7a8b8188de 89 }
NaotoMorita 73:be7a8b8188de 90
NaotoMorita 73:be7a8b8188de 91 int result = i2c.write(address, i2cBuffer, size + 2, false);
NaotoMorita 73:be7a8b8188de 92 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 93 }
NaotoMorita 73:be7a8b8188de 94
NaotoMorita 73:be7a8b8188de 95 // this function has no read limit
NaotoMorita 73:be7a8b8188de 96 void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 97 {
NaotoMorita 73:be7a8b8188de 98 char i2cBuffer[2];
NaotoMorita 73:be7a8b8188de 99 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 100 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 101
NaotoMorita 73:be7a8b8188de 102 // Reset eeprom pointer address
NaotoMorita 73:be7a8b8188de 103 int result = i2c.write(address, i2cBuffer, 2, false);
NaotoMorita 73:be7a8b8188de 104
NaotoMorita 73:be7a8b8188de 105 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 106
NaotoMorita 73:be7a8b8188de 107 // Read eeprom
NaotoMorita 73:be7a8b8188de 108 i2c.read(address, data, size);
NaotoMorita 73:be7a8b8188de 109 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 110 }