IRC Helicopter "SWIFT" Propo codec(Decode only) library
Dependents: SwiftPropoIR_TestProgram spinner2
DecodeSwift.cpp@0:8ceeb99b4c21, 2013-06-23 (annotated)
- Committer:
- suupen
- Date:
- Sun Jun 23 07:25:04 2013 +0000
- Revision:
- 0:8ceeb99b4c21
IRC Helicpoter "SWIFT" Propo codec(decode only) library
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
suupen | 0:8ceeb99b4c21 | 1 | /** |
suupen | 0:8ceeb99b4c21 | 2 | * IR Propo Decode class |
suupen | 0:8ceeb99b4c21 | 3 | * Version 0.0 130623 |
suupen | 0:8ceeb99b4c21 | 4 | * |
suupen | 0:8ceeb99b4c21 | 5 | * Writer:suupen |
suupen | 0:8ceeb99b4c21 | 6 | */ |
suupen | 0:8ceeb99b4c21 | 7 | #include "CodecSwift.h" |
suupen | 0:8ceeb99b4c21 | 8 | #include "DecodeSwift.h" |
suupen | 0:8ceeb99b4c21 | 9 | |
suupen | 0:8ceeb99b4c21 | 10 | /** |
suupen | 0:8ceeb99b4c21 | 11 | * Constructor DecodeSwift |
suupen | 0:8ceeb99b4c21 | 12 | * |
suupen | 0:8ceeb99b4c21 | 13 | */ |
suupen | 0:8ceeb99b4c21 | 14 | DecodeSwift::DecodeSwift(){ |
suupen | 0:8ceeb99b4c21 | 15 | // nothing |
suupen | 0:8ceeb99b4c21 | 16 | } |
suupen | 0:8ceeb99b4c21 | 17 | |
suupen | 0:8ceeb99b4c21 | 18 | /** |
suupen | 0:8ceeb99b4c21 | 19 | * Destructor. |
suupen | 0:8ceeb99b4c21 | 20 | */ |
suupen | 0:8ceeb99b4c21 | 21 | DecodeSwift::~DecodeSwift(){ |
suupen | 0:8ceeb99b4c21 | 22 | // nothing |
suupen | 0:8ceeb99b4c21 | 23 | } |
suupen | 0:8ceeb99b4c21 | 24 | |
suupen | 0:8ceeb99b4c21 | 25 | |
suupen | 0:8ceeb99b4c21 | 26 | /** |
suupen | 0:8ceeb99b4c21 | 27 | * Get swift controller data. |
suupen | 0:8ceeb99b4c21 | 28 | * |
suupen | 0:8ceeb99b4c21 | 29 | * @param data Pointer to propo recive data. |
suupen | 0:8ceeb99b4c21 | 30 | * |
suupen | 0:8ceeb99b4c21 | 31 | * @param swift Pointer to swift. |
suupen | 0:8ceeb99b4c21 | 32 | * |
suupen | 0:8ceeb99b4c21 | 33 | * @return receive answer : ture:recive ok false:recive ng. |
suupen | 0:8ceeb99b4c21 | 34 | */ |
suupen | 0:8ceeb99b4c21 | 35 | bool DecodeSwift::decode(uint8_t *data, CodecSwift::swiftPropo_t *swift) { |
suupen | 0:8ceeb99b4c21 | 36 | bool ans = false; |
suupen | 0:8ceeb99b4c21 | 37 | int8_t wk1 = 0; |
suupen | 0:8ceeb99b4c21 | 38 | int8_t wk2; |
suupen | 0:8ceeb99b4c21 | 39 | |
suupen | 0:8ceeb99b4c21 | 40 | int8_t wk3; |
suupen | 0:8ceeb99b4c21 | 41 | int8_t trimWork; |
suupen | 0:8ceeb99b4c21 | 42 | |
suupen | 0:8ceeb99b4c21 | 43 | // parity calculate |
suupen | 0:8ceeb99b4c21 | 44 | |
suupen | 0:8ceeb99b4c21 | 45 | for(uint8_t c=0; c < 3; c++){ |
suupen | 0:8ceeb99b4c21 | 46 | wk1 += data[c] & 0x3f; |
suupen | 0:8ceeb99b4c21 | 47 | } |
suupen | 0:8ceeb99b4c21 | 48 | if(0 != (wk1 & 0x0f)){ |
suupen | 0:8ceeb99b4c21 | 49 | wk1 = wk1 + 0x10 - 0x01; |
suupen | 0:8ceeb99b4c21 | 50 | } |
suupen | 0:8ceeb99b4c21 | 51 | else{ |
suupen | 0:8ceeb99b4c21 | 52 | wk1 |= 0x0f; |
suupen | 0:8ceeb99b4c21 | 53 | } |
suupen | 0:8ceeb99b4c21 | 54 | |
suupen | 0:8ceeb99b4c21 | 55 | wk1 &= 0x3f; |
suupen | 0:8ceeb99b4c21 | 56 | wk2 = data[3] & 0x3f; |
suupen | 0:8ceeb99b4c21 | 57 | if(wk1 == wk2){ |
suupen | 0:8ceeb99b4c21 | 58 | // parity ok |
suupen | 0:8ceeb99b4c21 | 59 | ans =true; |
suupen | 0:8ceeb99b4c21 | 60 | swift->count++; // recive count [1/1 [recive]/count] |
suupen | 0:8ceeb99b4c21 | 61 | swift->band = (data[3] >> 6 ) & 0x03; // 0x03:A band 0x02:B 0x01:C |
suupen | 0:8ceeb99b4c21 | 62 | swift->slottle = data[0] & 0x7f; // 0x00 - 0x72 |
suupen | 0:8ceeb99b4c21 | 63 | |
suupen | 0:8ceeb99b4c21 | 64 | trimWork = data[2] & 0x1F; |
suupen | 0:8ceeb99b4c21 | 65 | if((data[2] & 0x20) == 0x20){ |
suupen | 0:8ceeb99b4c21 | 66 | trimWork = -trimWork; |
suupen | 0:8ceeb99b4c21 | 67 | } |
suupen | 0:8ceeb99b4c21 | 68 | else{ |
suupen | 0:8ceeb99b4c21 | 69 | // nothine |
suupen | 0:8ceeb99b4c21 | 70 | } |
suupen | 0:8ceeb99b4c21 | 71 | swift->trim = trimWork; |
suupen | 0:8ceeb99b4c21 | 72 | |
suupen | 0:8ceeb99b4c21 | 73 | // right:1 to 15, left:-1 to -15 |
suupen | 0:8ceeb99b4c21 | 74 | wk3 = (data[1] >> 4) & 0x0f; |
suupen | 0:8ceeb99b4c21 | 75 | if(0x80 == (data[2] & 0x80)){ |
suupen | 0:8ceeb99b4c21 | 76 | swift->ladder = wk3; |
suupen | 0:8ceeb99b4c21 | 77 | } |
suupen | 0:8ceeb99b4c21 | 78 | else{ |
suupen | 0:8ceeb99b4c21 | 79 | swift->ladder = -wk3; |
suupen | 0:8ceeb99b4c21 | 80 | } |
suupen | 0:8ceeb99b4c21 | 81 | |
suupen | 0:8ceeb99b4c21 | 82 | // forward: 1 to 15 , backward -1 to -15 |
suupen | 0:8ceeb99b4c21 | 83 | wk3 = (data[1] & 0x0f); |
suupen | 0:8ceeb99b4c21 | 84 | if(0x40 == (data[2] & 0x40)){ |
suupen | 0:8ceeb99b4c21 | 85 | swift->elevator = wk3; |
suupen | 0:8ceeb99b4c21 | 86 | } |
suupen | 0:8ceeb99b4c21 | 87 | else{ |
suupen | 0:8ceeb99b4c21 | 88 | swift->elevator = -wk3; |
suupen | 0:8ceeb99b4c21 | 89 | } |
suupen | 0:8ceeb99b4c21 | 90 | } |
suupen | 0:8ceeb99b4c21 | 91 | else{ |
suupen | 0:8ceeb99b4c21 | 92 | // parity ng |
suupen | 0:8ceeb99b4c21 | 93 | |
suupen | 0:8ceeb99b4c21 | 94 | ans = false; |
suupen | 0:8ceeb99b4c21 | 95 | } |
suupen | 0:8ceeb99b4c21 | 96 | |
suupen | 0:8ceeb99b4c21 | 97 | |
suupen | 0:8ceeb99b4c21 | 98 | return ans; |
suupen | 0:8ceeb99b4c21 | 99 | |
suupen | 0:8ceeb99b4c21 | 100 | } |
suupen | 0:8ceeb99b4c21 | 101 | |
suupen | 0:8ceeb99b4c21 | 102 | |
suupen | 0:8ceeb99b4c21 | 103 | /** |
suupen | 0:8ceeb99b4c21 | 104 | * Normalize Swift data. |
suupen | 0:8ceeb99b4c21 | 105 | * |
suupen | 0:8ceeb99b4c21 | 106 | * @param buf Pointer to propo recive data. |
suupen | 0:8ceeb99b4c21 | 107 | * |
suupen | 0:8ceeb99b4c21 | 108 | * @param propo normalize propo data. |
suupen | 0:8ceeb99b4c21 | 109 | * |
suupen | 0:8ceeb99b4c21 | 110 | * @return receive answer : ture:recive ok false:recive ng. |
suupen | 0:8ceeb99b4c21 | 111 | */ |
suupen | 0:8ceeb99b4c21 | 112 | bool DecodeSwift::normalize(uint8_t *buf, CodecSwift::normalizePropo_t *propo) { |
suupen | 0:8ceeb99b4c21 | 113 | bool ans = false; |
suupen | 0:8ceeb99b4c21 | 114 | bool decodeCheck =false; |
suupen | 0:8ceeb99b4c21 | 115 | |
suupen | 0:8ceeb99b4c21 | 116 | CodecSwift::swiftPropo_t swift; |
suupen | 0:8ceeb99b4c21 | 117 | decodeCheck = DecodeSwift::decode(buf, &swift); |
suupen | 0:8ceeb99b4c21 | 118 | |
suupen | 0:8ceeb99b4c21 | 119 | if(decodeCheck == true){ |
suupen | 0:8ceeb99b4c21 | 120 | ans = true; |
suupen | 0:8ceeb99b4c21 | 121 | |
suupen | 0:8ceeb99b4c21 | 122 | propo->count++;// = swift.count; |
suupen | 0:8ceeb99b4c21 | 123 | |
suupen | 0:8ceeb99b4c21 | 124 | switch(swift.band){ |
suupen | 0:8ceeb99b4c21 | 125 | case CodecSwift::BAND_A: |
suupen | 0:8ceeb99b4c21 | 126 | propo->band = 1; |
suupen | 0:8ceeb99b4c21 | 127 | break; |
suupen | 0:8ceeb99b4c21 | 128 | case CodecSwift::BAND_B: |
suupen | 0:8ceeb99b4c21 | 129 | propo->band = 2; |
suupen | 0:8ceeb99b4c21 | 130 | break; |
suupen | 0:8ceeb99b4c21 | 131 | case CodecSwift::BAND_C: |
suupen | 0:8ceeb99b4c21 | 132 | propo->band = 3; |
suupen | 0:8ceeb99b4c21 | 133 | break; |
suupen | 0:8ceeb99b4c21 | 134 | default: |
suupen | 0:8ceeb99b4c21 | 135 | propo->band = 0; |
suupen | 0:8ceeb99b4c21 | 136 | break; |
suupen | 0:8ceeb99b4c21 | 137 | } |
suupen | 0:8ceeb99b4c21 | 138 | |
suupen | 0:8ceeb99b4c21 | 139 | if(swift.slottle > CodecSwift::SLOTTLE_MAX){swift.slottle = CodecSwift::SLOTTLE_MAX;} |
suupen | 0:8ceeb99b4c21 | 140 | propo->slottle = (float)swift.slottle / CodecSwift::SLOTTLE_MAX; |
suupen | 0:8ceeb99b4c21 | 141 | |
suupen | 0:8ceeb99b4c21 | 142 | if(swift.ladder >= 0){ |
suupen | 0:8ceeb99b4c21 | 143 | if(swift.ladder > CodecSwift::LADDER_MAX){swift.ladder = CodecSwift::LADDER_MAX;} |
suupen | 0:8ceeb99b4c21 | 144 | propo->ladder = (float)swift.ladder / CodecSwift::LADDER_MAX; |
suupen | 0:8ceeb99b4c21 | 145 | } |
suupen | 0:8ceeb99b4c21 | 146 | else{ |
suupen | 0:8ceeb99b4c21 | 147 | if(swift.ladder < CodecSwift::LADDER_MIN){swift.ladder = CodecSwift::LADDER_MIN;} |
suupen | 0:8ceeb99b4c21 | 148 | propo->ladder = -((float)swift.ladder / CodecSwift::LADDER_MIN); |
suupen | 0:8ceeb99b4c21 | 149 | } |
suupen | 0:8ceeb99b4c21 | 150 | |
suupen | 0:8ceeb99b4c21 | 151 | if(swift.elevator >= 0){ |
suupen | 0:8ceeb99b4c21 | 152 | if(swift.elevator > CodecSwift::ELEVATOR_MAX){swift.elevator = CodecSwift::ELEVATOR_MAX;} |
suupen | 0:8ceeb99b4c21 | 153 | propo->elevator = (float)swift.elevator / CodecSwift::ELEVATOR_MAX; |
suupen | 0:8ceeb99b4c21 | 154 | } |
suupen | 0:8ceeb99b4c21 | 155 | else{ |
suupen | 0:8ceeb99b4c21 | 156 | if(swift.elevator < CodecSwift::ELEVATOR_MIN){swift.elevator = CodecSwift::ELEVATOR_MIN;} |
suupen | 0:8ceeb99b4c21 | 157 | propo->elevator = -((float)swift.elevator / CodecSwift::ELEVATOR_MIN); |
suupen | 0:8ceeb99b4c21 | 158 | } |
suupen | 0:8ceeb99b4c21 | 159 | |
suupen | 0:8ceeb99b4c21 | 160 | if(swift.trim >=0 ){ |
suupen | 0:8ceeb99b4c21 | 161 | if(swift.trim > CodecSwift::TRIM_MAX){swift.trim = CodecSwift::TRIM_MAX;} |
suupen | 0:8ceeb99b4c21 | 162 | propo->trim = (float)swift.trim / CodecSwift::TRIM_MAX; |
suupen | 0:8ceeb99b4c21 | 163 | } |
suupen | 0:8ceeb99b4c21 | 164 | else{ |
suupen | 0:8ceeb99b4c21 | 165 | if(swift.trim < CodecSwift::TRIM_MIN){swift.trim = CodecSwift::TRIM_MIN;} |
suupen | 0:8ceeb99b4c21 | 166 | propo->trim = -((float)swift.trim / CodecSwift::TRIM_MIN); |
suupen | 0:8ceeb99b4c21 | 167 | } |
suupen | 0:8ceeb99b4c21 | 168 | } |
suupen | 0:8ceeb99b4c21 | 169 | else{ |
suupen | 0:8ceeb99b4c21 | 170 | // NG |
suupen | 0:8ceeb99b4c21 | 171 | ans = false; |
suupen | 0:8ceeb99b4c21 | 172 | } |
suupen | 0:8ceeb99b4c21 | 173 | |
suupen | 0:8ceeb99b4c21 | 174 | return ans; |
suupen | 0:8ceeb99b4c21 | 175 | } |
suupen | 0:8ceeb99b4c21 | 176 | |
suupen | 0:8ceeb99b4c21 | 177 | |
suupen | 0:8ceeb99b4c21 | 178 |