HAPSRG / SBUS_without_mainfile

SBUSのcppとhppだけ（フォーク）

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

SBUS.cpp@2:3241f9311a0e, 2021-03-03 (annotated)

Committer:
NaotoMorita
Date:
Wed Mar 03 03:32:30 2021 +0000
Revision:
2:3241f9311a0e
Parent:
1:b2a305158f89 
quaterinon attitude determination

Who changed what in which revision?

UserRevisionLine numberNew contents of line
surpace0924 0:072c4589bc62 1 #include "mbed.h"
surpace0924 0:072c4589bc62 2 #include "SBUS.hpp"
surpace0924 0:072c4589bc62 3
surpace0924 0:072c4589bc62 4 SBUS::SBUS(PinName tx, PinName rx) : sbus_(tx, rx)
surpace0924 0:072c4589bc62 5 {
surpace0924 0:072c4589bc62 6 sbusIndex = 0;
surpace0924 0:072c4589bc62 7 for (int i = 0; i < 18; i++)
surpace0924 0:072c4589bc62 8 rcChannel[i] = 0;
surpace0924 0:072c4589bc62 9
surpace0924 0:072c4589bc62 10 failSafe = true;
surpace0924 0:072c4589bc62 11
surpace0924 0:072c4589bc62 12 sbus_.baud(100000);
surpace0924 0:072c4589bc62 13 sbus_.format(8, Serial::Even, 2);
surpace0924 0:072c4589bc62 14 sbus_.attach(this, &SBUS::sbusIrqRx, Serial::RxIrq);
surpace0924 0:072c4589bc62 15 }
surpace0924 0:072c4589bc62 16
surpace0924 0:072c4589bc62 17 void SBUS::sbusIrqRx ()
surpace0924 0:072c4589bc62 18 {
surpace0924 0:072c4589bc62 19 static uint8_t sbus[25] = {0};
surpace0924 0:072c4589bc62 20
surpace0924 0:072c4589bc62 21 int val = sbus_.getc();
surpace0924 0:072c4589bc62 22
surpace0924 0:072c4589bc62 23
surpace0924 0:072c4589bc62 24 if (sbusIndex == 0 && val != 0x0F) {
NaotoMorita 2:3241f9311a0e 25 failSafe = true;
surpace0924 0:072c4589bc62 26 return;
surpace0924 0:072c4589bc62 27 }
surpace0924 0:072c4589bc62 28
surpace0924 0:072c4589bc62 29 sbus[sbusIndex] = val;
surpace0924 0:072c4589bc62 30 sbusIndex++;
surpace0924 0:072c4589bc62 31
surpace0924 0:072c4589bc62 32
surpace0924 0:072c4589bc62 33 if (sbusIndex == 25) {
surpace0924 0:072c4589bc62 34 sbusIndex = 0;
surpace0924 0:072c4589bc62 35
surpace0924 0:072c4589bc62 36 if (sbus[24] != 0x0) {
surpace0924 0:072c4589bc62 37 rcChannel[0] = ((sbus[1] | sbus[2] << 8) & 0x07FF);
surpace0924 0:072c4589bc62 38 rcChannel[1] = ((sbus[2] >> 3 | sbus[3] << 5) & 0x07FF);
surpace0924 0:072c4589bc62 39 rcChannel[2] = ((sbus[3] >> 6 | sbus[4] << 2 | sbus[5] << 10) & 0x07FF);
surpace0924 0:072c4589bc62 40 rcChannel[3] = ((sbus[5] >> 1 | sbus[6] << 7) & 0x07FF);
surpace0924 0:072c4589bc62 41 rcChannel[4] = ((sbus[6] >> 4 | sbus[7] << 4) & 0x07FF);
surpace0924 0:072c4589bc62 42 rcChannel[5] = ((sbus[7] >> 7 | sbus[8] << 1 | sbus[9] << 9) & 0x07FF);
surpace0924 0:072c4589bc62 43 rcChannel[6] = ((sbus[9] >> 2 | sbus[10] << 6) & 0x07FF);
surpace0924 0:072c4589bc62 44 rcChannel[7] = ((sbus[10] >> 5 | sbus[11] << 3) & 0x07FF);
surpace0924 0:072c4589bc62 45 rcChannel[8] = ((sbus[12] | sbus[13] << 8) & 0x07FF);
surpace0924 0:072c4589bc62 46 rcChannel[9] = ((sbus[13] >> 3 | sbus[14] << 5) & 0x07FF);
surpace0924 0:072c4589bc62 47 rcChannel[10] = ((sbus[14] >> 6 | sbus[15] << 2 | sbus[16] << 10) & 0x07FF);
surpace0924 0:072c4589bc62 48 rcChannel[11] = ((sbus[16] >> 1 | sbus[17] << 7) & 0x07FF);
surpace0924 0:072c4589bc62 49 rcChannel[12] = ((sbus[17] >> 4 | sbus[18] << 4) & 0x07FF);
surpace0924 0:072c4589bc62 50 rcChannel[13] = ((sbus[18] >> 7 | sbus[19] << 1 | sbus[20] << 9) & 0x07FF);
surpace0924 0:072c4589bc62 51 rcChannel[14] = ((sbus[20] >> 2 | sbus[21] << 6) & 0x07FF);
surpace0924 0:072c4589bc62 52 rcChannel[15] = ((sbus[21] >> 5 | sbus[22] << 3) & 0x07FF);
surpace0924 0:072c4589bc62 53
surpace0924 0:072c4589bc62 54 if ((sbus[23] >> 3) & 0x0001) {
surpace0924 0:072c4589bc62 55 // コントローラがOFF状態
surpace0924 0:072c4589bc62 56 failSafe = true;
surpace0924 0:072c4589bc62 57 } else {
surpace0924 0:072c4589bc62 58 failSafe = false;
surpace0924 0:072c4589bc62 59 }
surpace0924 0:072c4589bc62 60 }
surpace0924 0:072c4589bc62 61 }
surpace0924 0:072c4589bc62 62 }
surpace0924 0:072c4589bc62 63
surpace0924 0:072c4589bc62 64
surpace0924 0:072c4589bc62 65
surpace0924 0:072c4589bc62 66 int16_t SBUS::getData(uint8_t ch)
surpace0924 0:072c4589bc62 67 {
surpace0924 0:072c4589bc62 68 return rcChannel[ch];
surpace0924 0:072c4589bc62 69 }
surpace0924 0:072c4589bc62 70
surpace0924 0:072c4589bc62 71 int SBUS::getStickVal(int axis)
surpace0924 0:072c4589bc62 72 {
surpace0924 0:072c4589bc62 73 if(!failSafe) {
surpace0924 0:072c4589bc62 74 switch (axis) {
surpace0924 0:072c4589bc62 75 case 0:
surpace0924 0:072c4589bc62 76 return SBUS::map(getData(3), 368, 1680, -255, 255);
surpace0924 0:072c4589bc62 77 case 1:
surpace0924 0:072c4589bc62 78 return SBUS::map(getData(1), 368, 1680, 255, -255);
surpace0924 0:072c4589bc62 79 case 2:
surpace0924 0:072c4589bc62 80 return SBUS::map(getData(0), 368, 1680, -255, 255);
surpace0924 0:072c4589bc62 81 case 3:
surpace0924 0:072c4589bc62 82 return SBUS::map(getData(2), 368, 1680, -255, 255);
surpace0924 0:072c4589bc62 83 default:
surpace0924 0:072c4589bc62 84 return 0;
surpace0924 0:072c4589bc62 85 }
surpace0924 0:072c4589bc62 86 } else return 0;
surpace0924 0:072c4589bc62 87 }
surpace0924 0:072c4589bc62 88
surpace0924 0:072c4589bc62 89 int SBUS::getSwitchVal(int parm)
surpace0924 0:072c4589bc62 90 {
surpace0924 0:072c4589bc62 91 if(!failSafe) {
surpace0924 0:072c4589bc62 92 if (parm == 0)
surpace0924 0:072c4589bc62 93 return SBUS::map(getData(4), 144, 1904, 0, 2);
surpace0924 0:072c4589bc62 94 else if (parm <= 4)
surpace0924 0:072c4589bc62 95 return SBUS::map(getData(parm + 5), 144, 1904, 0, 2);
surpace0924 0:072c4589bc62 96 else
surpace0924 0:072c4589bc62 97 return 0;
surpace0924 0:072c4589bc62 98 } else return 3;
surpace0924 0:072c4589bc62 99 }
surpace0924 0:072c4589bc62 100
surpace0924 0:072c4589bc62 101 long SBUS::map(long x, long in_min, long in_max, long out_min, long out_max)
surpace0924 0:072c4589bc62 102 {
surpace0924 0:072c4589bc62 103 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
surpace0924 0:072c4589bc62 104 }