SBUSのcppとhppだけ(フォーク)
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
SBUS/SBUS.cpp@0:072c4589bc62, 2018-02-03 (annotated)
- Committer:
- surpace0924
- Date:
- Sat Feb 03 15:12:59 2018 +0000
- Revision:
- 0:072c4589bc62
1.1.0; ??????????
Who changed what in which revision?
User | Revision | Line number | New 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) { |
surpace0924 | 0:072c4589bc62 | 25 | return; |
surpace0924 | 0:072c4589bc62 | 26 | } |
surpace0924 | 0:072c4589bc62 | 27 | |
surpace0924 | 0:072c4589bc62 | 28 | sbus[sbusIndex] = val; |
surpace0924 | 0:072c4589bc62 | 29 | sbusIndex++; |
surpace0924 | 0:072c4589bc62 | 30 | |
surpace0924 | 0:072c4589bc62 | 31 | |
surpace0924 | 0:072c4589bc62 | 32 | if (sbusIndex == 25) { |
surpace0924 | 0:072c4589bc62 | 33 | sbusIndex = 0; |
surpace0924 | 0:072c4589bc62 | 34 | |
surpace0924 | 0:072c4589bc62 | 35 | if (sbus[24] != 0x0) { |
surpace0924 | 0:072c4589bc62 | 36 | rcChannel[0] = ((sbus[1] | sbus[2] << 8) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 37 | rcChannel[1] = ((sbus[2] >> 3 | sbus[3] << 5) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 38 | rcChannel[2] = ((sbus[3] >> 6 | sbus[4] << 2 | sbus[5] << 10) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 39 | rcChannel[3] = ((sbus[5] >> 1 | sbus[6] << 7) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 40 | rcChannel[4] = ((sbus[6] >> 4 | sbus[7] << 4) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 41 | rcChannel[5] = ((sbus[7] >> 7 | sbus[8] << 1 | sbus[9] << 9) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 42 | rcChannel[6] = ((sbus[9] >> 2 | sbus[10] << 6) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 43 | rcChannel[7] = ((sbus[10] >> 5 | sbus[11] << 3) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 44 | rcChannel[8] = ((sbus[12] | sbus[13] << 8) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 45 | rcChannel[9] = ((sbus[13] >> 3 | sbus[14] << 5) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 46 | rcChannel[10] = ((sbus[14] >> 6 | sbus[15] << 2 | sbus[16] << 10) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 47 | rcChannel[11] = ((sbus[16] >> 1 | sbus[17] << 7) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 48 | rcChannel[12] = ((sbus[17] >> 4 | sbus[18] << 4) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 49 | rcChannel[13] = ((sbus[18] >> 7 | sbus[19] << 1 | sbus[20] << 9) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 50 | rcChannel[14] = ((sbus[20] >> 2 | sbus[21] << 6) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 51 | rcChannel[15] = ((sbus[21] >> 5 | sbus[22] << 3) & 0x07FF); |
surpace0924 | 0:072c4589bc62 | 52 | |
surpace0924 | 0:072c4589bc62 | 53 | if ((sbus[23] >> 3) & 0x0001) { |
surpace0924 | 0:072c4589bc62 | 54 | // コントローラがOFF状態 |
surpace0924 | 0:072c4589bc62 | 55 | failSafe = true; |
surpace0924 | 0:072c4589bc62 | 56 | } else { |
surpace0924 | 0:072c4589bc62 | 57 | failSafe = false; |
surpace0924 | 0:072c4589bc62 | 58 | } |
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 | int16_t SBUS::getData(uint8_t ch) |
surpace0924 | 0:072c4589bc62 | 66 | { |
surpace0924 | 0:072c4589bc62 | 67 | return rcChannel[ch]; |
surpace0924 | 0:072c4589bc62 | 68 | } |
surpace0924 | 0:072c4589bc62 | 69 | |
surpace0924 | 0:072c4589bc62 | 70 | int SBUS::getStickVal(int axis) |
surpace0924 | 0:072c4589bc62 | 71 | { |
surpace0924 | 0:072c4589bc62 | 72 | if(!failSafe) { |
surpace0924 | 0:072c4589bc62 | 73 | switch (axis) { |
surpace0924 | 0:072c4589bc62 | 74 | case 0: |
surpace0924 | 0:072c4589bc62 | 75 | return SBUS::map(getData(3), 368, 1680, -255, 255); |
surpace0924 | 0:072c4589bc62 | 76 | case 1: |
surpace0924 | 0:072c4589bc62 | 77 | return SBUS::map(getData(1), 368, 1680, 255, -255); |
surpace0924 | 0:072c4589bc62 | 78 | case 2: |
surpace0924 | 0:072c4589bc62 | 79 | return SBUS::map(getData(0), 368, 1680, -255, 255); |
surpace0924 | 0:072c4589bc62 | 80 | case 3: |
surpace0924 | 0:072c4589bc62 | 81 | return SBUS::map(getData(2), 368, 1680, -255, 255); |
surpace0924 | 0:072c4589bc62 | 82 | default: |
surpace0924 | 0:072c4589bc62 | 83 | return 0; |
surpace0924 | 0:072c4589bc62 | 84 | } |
surpace0924 | 0:072c4589bc62 | 85 | } else return 0; |
surpace0924 | 0:072c4589bc62 | 86 | } |
surpace0924 | 0:072c4589bc62 | 87 | |
surpace0924 | 0:072c4589bc62 | 88 | int SBUS::getSwitchVal(int parm) |
surpace0924 | 0:072c4589bc62 | 89 | { |
surpace0924 | 0:072c4589bc62 | 90 | if(!failSafe) { |
surpace0924 | 0:072c4589bc62 | 91 | if (parm == 0) |
surpace0924 | 0:072c4589bc62 | 92 | return SBUS::map(getData(4), 144, 1904, 0, 2); |
surpace0924 | 0:072c4589bc62 | 93 | else if (parm <= 4) |
surpace0924 | 0:072c4589bc62 | 94 | return SBUS::map(getData(parm + 5), 144, 1904, 0, 2); |
surpace0924 | 0:072c4589bc62 | 95 | else |
surpace0924 | 0:072c4589bc62 | 96 | return 0; |
surpace0924 | 0:072c4589bc62 | 97 | } else return 3; |
surpace0924 | 0:072c4589bc62 | 98 | } |
surpace0924 | 0:072c4589bc62 | 99 | |
surpace0924 | 0:072c4589bc62 | 100 | long SBUS::map(long x, long in_min, long in_max, long out_min, long out_max) |
surpace0924 | 0:072c4589bc62 | 101 | { |
surpace0924 | 0:072c4589bc62 | 102 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
surpace0924 | 0:072c4589bc62 | 103 | } |