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@99:37cfd8633c9b, 2022-02-09 (annotated)

Committer:
osaka
Date:
Wed Feb 09 09:40:03 2022 +0000
Revision:
99:37cfd8633c9b
Parent:
97:2a65f22488e4
Child:
105:aaaed895ffaf
valuepack updated

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);
osaka 96:0be0d80d5299 25 Matrix vihat = eskf.getVihat();
osaka 96:0be0d80d5299 26 vp.vi[0] = vihat(1, 1);
osaka 96:0be0d80d5299 27 vp.vi[1] = vihat(2, 1);
osaka 96:0be0d80d5299 28 vp.vi[2] = vihat(3, 1);
osaka 91:393b9ae62681 29 if (prefligt_finished)
osaka 91:393b9ae62681 30 {
osaka 91:393b9ae62681 31 vp.readyFlag = 1;
osaka 91:393b9ae62681 32 }
osaka 91:393b9ae62681 33 else
osaka 91:393b9ae62681 34 {
osaka 91:393b9ae62681 35 vp.readyFlag = 0;
osaka 91:393b9ae62681 36 }
NaotoMorita 90:0b1f62f7a83a 37 if(updateValues.calibrationFlag == 1111){
NaotoMorita 90:0b1f62f7a83a 38 vp.calibEndFlag = 2;
NaotoMorita 90:0b1f62f7a83a 39 }else{
NaotoMorita 90:0b1f62f7a83a 40 vp.calibEndFlag = 0;
NaotoMorita 90:0b1f62f7a83a 41 }
NaotoMorita 73:be7a8b8188de 42 for(int i = 0 ; i<5 ; i++){
NaotoMorita 73:be7a8b8188de 43 vp.rc[i] = rc[i];
NaotoMorita 73:be7a8b8188de 44 }
osaka 99:37cfd8633c9b 45 vp.att_dt = att_dt;
osaka 99:37cfd8633c9b 46
NaotoMorita 73:be7a8b8188de 47 tail.Send(tail_address[pos_tail], &(posValues));
NaotoMorita 93:b0f7a1e91476 48 pc.printf("%f %f %f %d %d\r\n",rpy.x/M_PI*180.0f,rpy.y/M_PI*180.0f,rpy.z/M_PI*180.0f,vp.readyFlag,updateValues.calibrationFlag);
NaotoMorita 73:be7a8b8188de 49 }
NaotoMorita 73:be7a8b8188de 50
NaotoMorita 73:be7a8b8188de 51 // eeprom書き込み・読み込みに必要な関数
NaotoMorita 73:be7a8b8188de 52 void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 53 {
NaotoMorita 73:be7a8b8188de 54 char i2cBuffer[size + 2];
NaotoMorita 73:be7a8b8188de 55 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 56 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 57
NaotoMorita 73:be7a8b8188de 58 for (int i = 0; i < size; i++) {
NaotoMorita 73:be7a8b8188de 59 i2cBuffer[i + 2] = data[i];
NaotoMorita 73:be7a8b8188de 60 }
NaotoMorita 73:be7a8b8188de 61
NaotoMorita 73:be7a8b8188de 62 int result = i2c.write(address, i2cBuffer, size + 2, false);
NaotoMorita 73:be7a8b8188de 63 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 64 }
NaotoMorita 73:be7a8b8188de 65
NaotoMorita 73:be7a8b8188de 66 // this function has no read limit
NaotoMorita 73:be7a8b8188de 67 void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 68 {
NaotoMorita 73:be7a8b8188de 69 char i2cBuffer[2];
NaotoMorita 73:be7a8b8188de 70 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 71 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 72
NaotoMorita 73:be7a8b8188de 73 // Reset eeprom pointer address
NaotoMorita 73:be7a8b8188de 74 int result = i2c.write(address, i2cBuffer, 2, false);
NaotoMorita 73:be7a8b8188de 75
NaotoMorita 73:be7a8b8188de 76 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 77
NaotoMorita 73:be7a8b8188de 78 // Read eeprom
NaotoMorita 73:be7a8b8188de 79 i2c.read(address, data, size);
NaotoMorita 73:be7a8b8188de 80 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 81 }