HAPSRG / UsaPack

usapack public

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

UsaPack.hpp@11:6e58df8fd7ce, 2021-06-28 (annotated)

Committer:
NaotoMorita
Date:
Mon Jun 28 01:40:08 2021 +0000
Revision:
11:6e58df8fd7ce
Parent:
10:a2c85eed720b 
errStateEKF

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 0:8adbff0ff843 1 #ifndef __USAPACK_HPP__
cocorlow 0:8adbff0ff843 2 #define __USAPACK_HPP__
cocorlow 0:8adbff0ff843 3
cocorlow 0:8adbff0ff843 4 #include "mbed.h"
cocorlow 0:8adbff0ff843 5
cocorlow 7:bc00f60af715 6 #define receive_size 256
cocorlow 7:bc00f60af715 7 #define package_types 64
cocorlow 7:bc00f60af715 8 #define send_size 256
cocorlow 7:bc00f60af715 9
cocorlow 0:8adbff0ff843 10 class UsaPack
cocorlow 0:8adbff0ff843 11 {
cocorlow 0:8adbff0ff843 12 private:
NaotoMorita 10:a2c85eed720b 13
cocorlow 3:526f824b50ef 14 char receive_buffer[receive_size];
cocorlow 3:526f824b50ef 15 volatile int receive_index;
cocorlow 3:526f824b50ef 16 volatile int package_index;
cocorlow 3:526f824b50ef 17 volatile int package_address[package_types];
cocorlow 3:526f824b50ef 18 volatile int package_size[package_types];
cocorlow 0:8adbff0ff843 19 void* package_object[package_types];
cocorlow 0:8adbff0ff843 20
cocorlow 3:526f824b50ef 21 char send_buffer[send_size];
cocorlow 3:526f824b50ef 22 volatile int send_index;
cocorlow 3:526f824b50ef 23 volatile int send_end_index;
cocorlow 3:526f824b50ef 24
NaotoMorita 10:a2c85eed720b 25
cocorlow 3:526f824b50ef 26 void SendByte();
cocorlow 1:7d6e751f5986 27 void Decode();
cocorlow 0:8adbff0ff843 28
cocorlow 0:8adbff0ff843 29 public:
cocorlow 0:8adbff0ff843 30 UsaPack(PinName tx, PinName rx, int baud);
NaotoMorita 10:a2c85eed720b 31 void Receive();
NaotoMorita 10:a2c85eed720b 32 Serial serial;
NaotoMorita 11:6e58df8fd7ce 33
cocorlow 1:7d6e751f5986 34 static void CobsEncode(char data[], int length, char send_data[]);
cocorlow 1:7d6e751f5986 35 static void CobsDecode(char receive_data[], int length, char data[]);
cocorlow 0:8adbff0ff843 36
cocorlow 0:8adbff0ff843 37 template <typename T>
cocorlow 0:8adbff0ff843 38 void Subscribe(int address, T* receive_data)
cocorlow 0:8adbff0ff843 39 {
cocorlow 0:8adbff0ff843 40 package_address[package_index] = address;
cocorlow 0:8adbff0ff843 41 package_size[package_index] = sizeof(T);
cocorlow 0:8adbff0ff843 42 package_object[package_index] = receive_data;
cocorlow 0:8adbff0ff843 43 package_index++;
cocorlow 0:8adbff0ff843 44 }
cocorlow 0:8adbff0ff843 45
cocorlow 0:8adbff0ff843 46 template <typename T>
cocorlow 9:7d39154143a1 47 void Send(int address, T* send_data)
cocorlow 0:8adbff0ff843 48 {
cocorlow 0:8adbff0ff843 49 union _package
cocorlow 0:8adbff0ff843 50 {
cocorlow 0:8adbff0ff843 51 struct
cocorlow 0:8adbff0ff843 52 {
cocorlow 0:8adbff0ff843 53 int _address;
cocorlow 4:844eff5a2fa8 54 int _checksum;
cocorlow 0:8adbff0ff843 55 T raw_data;
cocorlow 0:8adbff0ff843 56 } address_package;
cocorlow 1:7d6e751f5986 57 char byte_data[sizeof(address_package)];
cocorlow 0:8adbff0ff843 58 };
cocorlow 1:7d6e751f5986 59 char send_cobs_data[sizeof(_package)+2];
cocorlow 0:8adbff0ff843 60
cocorlow 0:8adbff0ff843 61 _package package;
cocorlow 0:8adbff0ff843 62 package.address_package._address = address;
cocorlow 1:7d6e751f5986 63 package.address_package.raw_data = *send_data;
cocorlow 4:844eff5a2fa8 64
cocorlow 4:844eff5a2fa8 65 int checksum = 0;
cocorlow 4:844eff5a2fa8 66 for (int i = 8; i < sizeof(_package); i++)
cocorlow 4:844eff5a2fa8 67 {
cocorlow 4:844eff5a2fa8 68 checksum += package.byte_data[i];
cocorlow 4:844eff5a2fa8 69 }
cocorlow 6:43078601fc5c 70 package.address_package._checksum = checksum;
cocorlow 4:844eff5a2fa8 71
cocorlow 1:7d6e751f5986 72 CobsEncode(package.byte_data, (int)sizeof(_package), send_cobs_data);
cocorlow 1:7d6e751f5986 73 for (int i = 0; i < sizeof(_package)+2; i++)
cocorlow 0:8adbff0ff843 74 {
cocorlow 3:526f824b50ef 75 send_buffer[(send_end_index + i) % send_size] = send_cobs_data[i];
cocorlow 0:8adbff0ff843 76 }
cocorlow 3:526f824b50ef 77 send_end_index = (send_end_index + sizeof(_package) + 2) % send_size;
cocorlow 3:526f824b50ef 78 serial.attach(this, &UsaPack::SendByte, Serial::TxIrq);
cocorlow 0:8adbff0ff843 79 }
cocorlow 0:8adbff0ff843 80
cocorlow 0:8adbff0ff843 81
cocorlow 0:8adbff0ff843 82
cocorlow 0:8adbff0ff843 83 };
cocorlow 0:8adbff0ff843 84
cocorlow 0:8adbff0ff843 85
cocorlow 0:8adbff0ff843 86 #endif