suu pen / CodecIRPropoSwift

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?

UserRevisionLine numberNew 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