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@73:be7a8b8188de, 2021-08-25 (annotated)

Committer:
NaotoMorita
Date:
Wed Aug 25 07:01:12 2021 +0000
Revision:
73:be7a8b8188de
Child:
75:7fa924da9d32
centered correction

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.day = updateValues.day;
NaotoMorita 73:be7a8b8188de 7 vp.ms = updateValues.ms;
NaotoMorita 73:be7a8b8188de 8 ekf.getGyroBias(vp.gyroBias, vp.gyroBiasCov);
NaotoMorita 73:be7a8b8188de 9 //ekf.getAccBias(vp.accBias, vp.accBiasCov);
NaotoMorita 73:be7a8b8188de 10 vp.acc[0] = acc.x;
NaotoMorita 73:be7a8b8188de 11 vp.acc[1] = acc.y;
NaotoMorita 73:be7a8b8188de 12 vp.acc[2] = acc.z;
NaotoMorita 73:be7a8b8188de 13 vp.gyro[0] = gyro.x;
NaotoMorita 73:be7a8b8188de 14 vp.gyro[1] = gyro.y;
NaotoMorita 73:be7a8b8188de 15 vp.gyro[2] = gyro.z;
NaotoMorita 73:be7a8b8188de 16 vp.mag[0] = mag.x;
NaotoMorita 73:be7a8b8188de 17 vp.mag[1] = mag.y;
NaotoMorita 73:be7a8b8188de 18 vp.mag[2] = mag.z;
NaotoMorita 73:be7a8b8188de 19 vp.rpy[0] = rpy.x;
NaotoMorita 73:be7a8b8188de 20 vp.rpy[1] = rpy.y;
NaotoMorita 73:be7a8b8188de 21 vp.rpy[2] = rpy.z;
NaotoMorita 73:be7a8b8188de 22 vp.de = de;
NaotoMorita 73:be7a8b8188de 23 vp.deobj = deobj;
NaotoMorita 73:be7a8b8188de 24 for(int i = 0 ; i<5 ; i++){
NaotoMorita 73:be7a8b8188de 25 vp.rc[i] = rc[i];
NaotoMorita 73:be7a8b8188de 26 }
NaotoMorita 73:be7a8b8188de 27 tail.Send(tail_address[pos_tail], &(posValues));
NaotoMorita 73:be7a8b8188de 28 ekf.updateCenteredGyroBiasCorrection(updateValues.gyroBias);
NaotoMorita 73:be7a8b8188de 29 //ekf.updateCenteredAccBiasCorrection(updateValues.accBias);
NaotoMorita 73:be7a8b8188de 30 pc.printf("%f %f\n\r",rpy.x*180.0f/M_PI,updateValues.gyroBias[0]);
NaotoMorita 73:be7a8b8188de 31 }
NaotoMorita 73:be7a8b8188de 32
NaotoMorita 73:be7a8b8188de 33 // eeprom書き込み・読み込みに必要な関数
NaotoMorita 73:be7a8b8188de 34 void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 35 {
NaotoMorita 73:be7a8b8188de 36 char i2cBuffer[size + 2];
NaotoMorita 73:be7a8b8188de 37 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 38 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 39
NaotoMorita 73:be7a8b8188de 40 for (int i = 0; i < size; i++) {
NaotoMorita 73:be7a8b8188de 41 i2cBuffer[i + 2] = data[i];
NaotoMorita 73:be7a8b8188de 42 }
NaotoMorita 73:be7a8b8188de 43
NaotoMorita 73:be7a8b8188de 44 int result = i2c.write(address, i2cBuffer, size + 2, false);
NaotoMorita 73:be7a8b8188de 45 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 46 }
NaotoMorita 73:be7a8b8188de 47
NaotoMorita 73:be7a8b8188de 48 // this function has no read limit
NaotoMorita 73:be7a8b8188de 49 void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 73:be7a8b8188de 50 {
NaotoMorita 73:be7a8b8188de 51 char i2cBuffer[2];
NaotoMorita 73:be7a8b8188de 52 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 73:be7a8b8188de 53 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 73:be7a8b8188de 54
NaotoMorita 73:be7a8b8188de 55 // Reset eeprom pointer address
NaotoMorita 73:be7a8b8188de 56 int result = i2c.write(address, i2cBuffer, 2, false);
NaotoMorita 73:be7a8b8188de 57
NaotoMorita 73:be7a8b8188de 58 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 59
NaotoMorita 73:be7a8b8188de 60 // Read eeprom
NaotoMorita 73:be7a8b8188de 61 i2c.read(address, data, size);
NaotoMorita 73:be7a8b8188de 62 //sleep_ms(500);
NaotoMorita 73:be7a8b8188de 63 }