Hepta 2
Hepta_Cam&GPS
Dependents: HEPTA2_assembly_0720 HEPTA2_ALL HEPTA2_ALL_ver0803_02
Fork of
HeptaSerial
by 
Hepta 2
HeptaSerial.cpp@6:c11f48bbb567, 2017-08-05 (annotated)
- Committer:
- hepta2ume
- Date:
- Sat Aug 05 13:36:04 2017 +0000
- Revision:
- 6:c11f48bbb567
- Parent:
- 5:50a03fcd9cc0
test
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| hepta2ume | 2:686c7043f5f4 | 1 | #include "HeptaSerial.h" |
| hepta2ume | 1:d16a4c3a272a | 2 | static FILE *fp_jpeg; |
| sunifu | 0:5a6468b4164d | 3 | #define WAITIDLE waitIdle |
| sunifu | 0:5a6468b4164d | 4 | #define SENDFUNC sendBytes |
| sunifu | 0:5a6468b4164d | 5 | #define RECVFUNC recvBytes |
| sunifu | 0:5a6468b4164d | 6 | #define WAITFUNC waitRecv |
| hepta2ume | 1:d16a4c3a272a | 7 | int num=0; |
| sunifu | 0:5a6468b4164d | 8 | /** |
| sunifu | 0:5a6468b4164d | 9 | * Constructor. |
| sunifu | 0:5a6468b4164d | 10 | * |
| sunifu | 0:5a6468b4164d | 11 | * @param tx A pin for transmit. |
| sunifu | 0:5a6468b4164d | 12 | * @param rx A pin for receive. |
| sunifu | 0:5a6468b4164d | 13 | * @param baud Baud rate. (Default is Baud14400.) |
| sunifu | 0:5a6468b4164d | 14 | */ |
| hepta2ume | 4:57c7f33a3621 | 15 | HeptaSerial::HeptaSerial(PinName tx, PinName rx) : serial(tx, rx) |
| hepta2ume | 1:d16a4c3a272a | 16 | { |
| hepta2ume | 3:12659e671bad | 17 | //serial.baud(baud); |
| sunifu | 0:5a6468b4164d | 18 | } |
| sunifu | 0:5a6468b4164d | 19 | |
| sunifu | 0:5a6468b4164d | 20 | /** |
| sunifu | 0:5a6468b4164d | 21 | * Destructor. |
| sunifu | 0:5a6468b4164d | 22 | */ |
| hepta2ume | 2:686c7043f5f4 | 23 | HeptaSerial::~HeptaSerial() |
| hepta2ume | 1:d16a4c3a272a | 24 | { |
| sunifu | 0:5a6468b4164d | 25 | } |
| sunifu | 0:5a6468b4164d | 26 | |
| sunifu | 0:5a6468b4164d | 27 | /** |
| sunifu | 0:5a6468b4164d | 28 | * Make a sync. for baud rate. |
| sunifu | 0:5a6468b4164d | 29 | */ |
| hepta2ume | 2:686c7043f5f4 | 30 | HeptaSerial::ErrorNumber HeptaSerial::sync() |
| hepta2ume | 1:d16a4c3a272a | 31 | { |
| sunifu | 0:5a6468b4164d | 32 | for (int i = 0; i < SYNCMAX; i++) { |
| sunifu | 0:5a6468b4164d | 33 | if (NoError == sendSync()) { |
| sunifu | 0:5a6468b4164d | 34 | if (NoError == recvAckOrNck()) { |
| sunifu | 0:5a6468b4164d | 35 | if (NoError == recvSync()) { |
| sunifu | 0:5a6468b4164d | 36 | if (NoError == sendAck(0x0D, 0x00)) { |
| sunifu | 0:5a6468b4164d | 37 | /* |
| sunifu | 0:5a6468b4164d | 38 | * After synchronization, the camera needs a little time for AEC and AGC to be stable. |
| sunifu | 0:5a6468b4164d | 39 | * Users should wait for 1-2 seconds before capturing the first picture. |
| sunifu | 0:5a6468b4164d | 40 | */ |
| sunifu | 0:5a6468b4164d | 41 | wait(2.0); |
| sunifu | 0:5a6468b4164d | 42 | return NoError; |
| sunifu | 0:5a6468b4164d | 43 | } |
| sunifu | 0:5a6468b4164d | 44 | } |
| sunifu | 0:5a6468b4164d | 45 | } |
| sunifu | 0:5a6468b4164d | 46 | } |
| sunifu | 0:5a6468b4164d | 47 | wait_ms(50); |
| sunifu | 0:5a6468b4164d | 48 | } |
| sunifu | 0:5a6468b4164d | 49 | return UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 50 | } |
| sunifu | 0:5a6468b4164d | 51 | |
| sunifu | 0:5a6468b4164d | 52 | /** |
| sunifu | 0:5a6468b4164d | 53 | * Initialize. |
| sunifu | 0:5a6468b4164d | 54 | * |
| sunifu | 0:5a6468b4164d | 55 | * @param baud Camera Interface Speed. |
| sunifu | 0:5a6468b4164d | 56 | * @param jr JPEG resolution. |
| sunifu | 0:5a6468b4164d | 57 | */ |
| hepta2ume | 2:686c7043f5f4 | 58 | HeptaSerial::ErrorNumber HeptaSerial::init(Baud baud,JpegResolution jr) |
| hepta2ume | 1:d16a4c3a272a | 59 | { |
| sunifu | 0:5a6468b4164d | 60 | int i ; |
| sunifu | 0:5a6468b4164d | 61 | ErrorNumber en; |
| sunifu | 0:5a6468b4164d | 62 | WAITIDLE(); |
| sunifu | 0:5a6468b4164d | 63 | setmbedBaud((Baud)(0x07)) ; |
| hepta2ume | 1:d16a4c3a272a | 64 | |
| sunifu | 0:5a6468b4164d | 65 | for ( i = 1 ; i < 7 ; i++ ) { |
| sunifu | 0:5a6468b4164d | 66 | if ( NoError == sendSync() ) { |
| hepta2ume | 1:d16a4c3a272a | 67 | if ( NoError == recvAckOrNck() ) { |
| sunifu | 0:5a6468b4164d | 68 | if ( NoError == recvSync() ) { |
| sunifu | 0:5a6468b4164d | 69 | if ( NoError == sendAck(0x0D, 0x00) ) { |
| sunifu | 0:5a6468b4164d | 70 | en = sendInitial(baud,jr); |
| sunifu | 0:5a6468b4164d | 71 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 72 | return en; |
| hepta2ume | 1:d16a4c3a272a | 73 | } |
| sunifu | 0:5a6468b4164d | 74 | en = recvAckOrNck(); |
| sunifu | 0:5a6468b4164d | 75 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 76 | return en; |
| hepta2ume | 1:d16a4c3a272a | 77 | } |
| sunifu | 0:5a6468b4164d | 78 | wait_ms(50) ; |
| hepta2ume | 1:d16a4c3a272a | 79 | setmbedBaud(baud); |
| hepta2ume | 1:d16a4c3a272a | 80 | //wait_ms(50) ; |
| sunifu | 0:5a6468b4164d | 81 | static bool alreadySetupPackageSize = false; |
| sunifu | 0:5a6468b4164d | 82 | if (!alreadySetupPackageSize) { |
| sunifu | 0:5a6468b4164d | 83 | en = sendSetPackageSize(packageSize); |
| sunifu | 0:5a6468b4164d | 84 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 85 | return en; |
| sunifu | 0:5a6468b4164d | 86 | } |
| sunifu | 0:5a6468b4164d | 87 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 88 | en = recvAckOrNck(); |
| sunifu | 0:5a6468b4164d | 89 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 90 | return en; |
| sunifu | 0:5a6468b4164d | 91 | } |
| sunifu | 0:5a6468b4164d | 92 | alreadySetupPackageSize = true; |
| sunifu | 0:5a6468b4164d | 93 | } |
| hepta2ume | 1:d16a4c3a272a | 94 | |
| sunifu | 0:5a6468b4164d | 95 | wait(2.0); |
| hepta2ume | 1:d16a4c3a272a | 96 | return (ErrorNumber)NoError; |
| hepta2ume | 1:d16a4c3a272a | 97 | /* |
| hepta2ume | 1:d16a4c3a272a | 98 | * After synchronization, the camera needs a little time for AEC and AGC to be stable. |
| hepta2ume | 1:d16a4c3a272a | 99 | * Users should wait for 1-2 seconds before capturing the first picture. |
| hepta2ume | 1:d16a4c3a272a | 100 | */ |
| sunifu | 0:5a6468b4164d | 101 | } |
| hepta2ume | 1:d16a4c3a272a | 102 | } |
| hepta2ume | 1:d16a4c3a272a | 103 | } else { |
| hepta2ume | 1:d16a4c3a272a | 104 | setmbedBaud((Baud)(i+1)) ; |
| sunifu | 0:5a6468b4164d | 105 | } |
| sunifu | 0:5a6468b4164d | 106 | } |
| hepta2ume | 1:d16a4c3a272a | 107 | wait_ms(50); |
| hepta2ume | 1:d16a4c3a272a | 108 | } |
| sunifu | 0:5a6468b4164d | 109 | return UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 110 | } |
| sunifu | 0:5a6468b4164d | 111 | |
| sunifu | 0:5a6468b4164d | 112 | |
| sunifu | 0:5a6468b4164d | 113 | /** |
| sunifu | 0:5a6468b4164d | 114 | * Get JPEG snapshot picture. |
| sunifu | 0:5a6468b4164d | 115 | * |
| sunifu | 0:5a6468b4164d | 116 | * @param func A pointer to a callback function. |
| sunifu | 0:5a6468b4164d | 117 | * You can block this function until saving the image datas. |
| sunifu | 0:5a6468b4164d | 118 | * @return Status of the error. |
| sunifu | 0:5a6468b4164d | 119 | */ |
| hepta2ume | 2:686c7043f5f4 | 120 | HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture() |
| hepta2ume | 1:d16a4c3a272a | 121 | { |
| hepta2ume | 1:d16a4c3a272a | 122 | WAITIDLE(); |
| hepta2ume | 1:d16a4c3a272a | 123 | ErrorNumber en; |
| hepta2ume | 1:d16a4c3a272a | 124 | |
| hepta2ume | 1:d16a4c3a272a | 125 | |
| hepta2ume | 1:d16a4c3a272a | 126 | en = sendSnapshot(); |
| hepta2ume | 1:d16a4c3a272a | 127 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 128 | return en; |
| hepta2ume | 1:d16a4c3a272a | 129 | } |
| hepta2ume | 1:d16a4c3a272a | 130 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 131 | en = recvAckOrNck(); |
| hepta2ume | 1:d16a4c3a272a | 132 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 133 | return en; |
| hepta2ume | 1:d16a4c3a272a | 134 | } |
| hepta2ume | 1:d16a4c3a272a | 135 | |
| hepta2ume | 1:d16a4c3a272a | 136 | en = sendGetPicture(); |
| hepta2ume | 1:d16a4c3a272a | 137 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 138 | return en; |
| hepta2ume | 1:d16a4c3a272a | 139 | } |
| hepta2ume | 1:d16a4c3a272a | 140 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 141 | en = recvAckOrNck(); |
| hepta2ume | 1:d16a4c3a272a | 142 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 143 | return en; |
| hepta2ume | 1:d16a4c3a272a | 144 | } |
| hepta2ume | 1:d16a4c3a272a | 145 | |
| hepta2ume | 1:d16a4c3a272a | 146 | /* |
| hepta2ume | 1:d16a4c3a272a | 147 | * Data : snapshot picture |
| hepta2ume | 1:d16a4c3a272a | 148 | */ |
| hepta2ume | 1:d16a4c3a272a | 149 | uint32_t length = 0; |
| hepta2ume | 1:d16a4c3a272a | 150 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 151 | en = recvData(&length); |
| hepta2ume | 1:d16a4c3a272a | 152 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 153 | return en; |
| hepta2ume | 1:d16a4c3a272a | 154 | } |
| hepta2ume | 1:d16a4c3a272a | 155 | en = sendAck(0x00, 0); |
| hepta2ume | 1:d16a4c3a272a | 156 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 157 | return en; |
| hepta2ume | 1:d16a4c3a272a | 158 | } |
| hepta2ume | 1:d16a4c3a272a | 159 | |
| hepta2ume | 1:d16a4c3a272a | 160 | char databuf[packageSize - 6]; |
| hepta2ume | 1:d16a4c3a272a | 161 | uint16_t pkg_total = length / (packageSize - 6); |
| hepta2ume | 1:d16a4c3a272a | 162 | for (int i = 0; i <= (int)pkg_total; i++) { |
| hepta2ume | 1:d16a4c3a272a | 163 | uint16_t checksum = 0; |
| hepta2ume | 1:d16a4c3a272a | 164 | // ID. |
| hepta2ume | 1:d16a4c3a272a | 165 | char idbuf[2]; |
| hepta2ume | 1:d16a4c3a272a | 166 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 167 | if (!RECVFUNC(idbuf, sizeof(idbuf))) { |
| hepta2ume | 1:d16a4c3a272a | 168 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 169 | } |
| hepta2ume | 1:d16a4c3a272a | 170 | checksum += idbuf[0]; |
| hepta2ume | 1:d16a4c3a272a | 171 | checksum += idbuf[1]; |
| hepta2ume | 1:d16a4c3a272a | 172 | uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0); |
| hepta2ume | 1:d16a4c3a272a | 173 | if (id != i) { |
| hepta2ume | 1:d16a4c3a272a | 174 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 175 | } |
| hepta2ume | 1:d16a4c3a272a | 176 | |
| hepta2ume | 1:d16a4c3a272a | 177 | // Size of the data. |
| hepta2ume | 1:d16a4c3a272a | 178 | char dsbuf[2]; |
| hepta2ume | 1:d16a4c3a272a | 179 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 180 | if (!RECVFUNC(dsbuf, sizeof(dsbuf))) { |
| hepta2ume | 1:d16a4c3a272a | 181 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 182 | } |
| hepta2ume | 1:d16a4c3a272a | 183 | |
| hepta2ume | 1:d16a4c3a272a | 184 | // Received the data. |
| hepta2ume | 1:d16a4c3a272a | 185 | checksum += dsbuf[0]; |
| hepta2ume | 1:d16a4c3a272a | 186 | checksum += dsbuf[1]; |
| hepta2ume | 1:d16a4c3a272a | 187 | uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0); |
| hepta2ume | 1:d16a4c3a272a | 188 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 189 | if (!RECVFUNC(&databuf[0], ds)) { |
| hepta2ume | 1:d16a4c3a272a | 190 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 191 | } |
| hepta2ume | 1:d16a4c3a272a | 192 | for (int j = 0; j < ds; j++) { |
| hepta2ume | 1:d16a4c3a272a | 193 | checksum += databuf[j]; |
| hepta2ume | 1:d16a4c3a272a | 194 | } |
| hepta2ume | 1:d16a4c3a272a | 195 | |
| hepta2ume | 1:d16a4c3a272a | 196 | // Verify code. |
| hepta2ume | 1:d16a4c3a272a | 197 | char vcbuf[2]; |
| hepta2ume | 1:d16a4c3a272a | 198 | WAITFUNC(); |
| hepta2ume | 1:d16a4c3a272a | 199 | if (!RECVFUNC(vcbuf, sizeof(vcbuf))) { |
| hepta2ume | 1:d16a4c3a272a | 200 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 201 | } |
| hepta2ume | 1:d16a4c3a272a | 202 | uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0); |
| hepta2ume | 1:d16a4c3a272a | 203 | if (vc != (checksum & 0xff)) { |
| hepta2ume | 1:d16a4c3a272a | 204 | return (ErrorNumber)UnexpectedReply; |
| hepta2ume | 1:d16a4c3a272a | 205 | } |
| hepta2ume | 1:d16a4c3a272a | 206 | |
| hepta2ume | 1:d16a4c3a272a | 207 | /* |
| hepta2ume | 1:d16a4c3a272a | 208 | * Call a call back function. |
| hepta2ume | 1:d16a4c3a272a | 209 | * You can block this function while working. |
| hepta2ume | 1:d16a4c3a272a | 210 | */ |
| hepta2ume | 1:d16a4c3a272a | 211 | size_t siz = ds; |
| hepta2ume | 1:d16a4c3a272a | 212 | for (int ii = 0; ii < (int)siz; ii++) { |
| hepta2ume | 1:d16a4c3a272a | 213 | fprintf(fp_jpeg, "%c", databuf[ii]); |
| hepta2ume | 1:d16a4c3a272a | 214 | } |
| hepta2ume | 1:d16a4c3a272a | 215 | /* |
| hepta2ume | 1:d16a4c3a272a | 216 | * We should wait for camera working before reply a ACK. |
| hepta2ume | 1:d16a4c3a272a | 217 | */ |
| hepta2ume | 1:d16a4c3a272a | 218 | wait_ms(100); |
| hepta2ume | 1:d16a4c3a272a | 219 | en = sendAck(0x00, 1 + i); |
| hepta2ume | 1:d16a4c3a272a | 220 | if (NoError != en) { |
| hepta2ume | 1:d16a4c3a272a | 221 | return en; |
| hepta2ume | 1:d16a4c3a272a | 222 | } |
| hepta2ume | 1:d16a4c3a272a | 223 | } |
| hepta2ume | 1:d16a4c3a272a | 224 | |
| hepta2ume | 1:d16a4c3a272a | 225 | return (ErrorNumber)NoError; |
| hepta2ume | 1:d16a4c3a272a | 226 | } |
| hepta2ume | 1:d16a4c3a272a | 227 | |
| hepta2ume | 2:686c7043f5f4 | 228 | HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture_data() |
| hepta2ume | 1:d16a4c3a272a | 229 | { |
| sunifu | 0:5a6468b4164d | 230 | WAITIDLE(); |
| sunifu | 0:5a6468b4164d | 231 | ErrorNumber en; |
| sunifu | 0:5a6468b4164d | 232 | |
| sunifu | 0:5a6468b4164d | 233 | |
| sunifu | 0:5a6468b4164d | 234 | en = sendSnapshot(); |
| sunifu | 0:5a6468b4164d | 235 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 236 | return en; |
| sunifu | 0:5a6468b4164d | 237 | } |
| sunifu | 0:5a6468b4164d | 238 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 239 | en = recvAckOrNck(); |
| sunifu | 0:5a6468b4164d | 240 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 241 | return en; |
| sunifu | 0:5a6468b4164d | 242 | } |
| sunifu | 0:5a6468b4164d | 243 | |
| sunifu | 0:5a6468b4164d | 244 | en = sendGetPicture(); |
| sunifu | 0:5a6468b4164d | 245 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 246 | return en; |
| sunifu | 0:5a6468b4164d | 247 | } |
| sunifu | 0:5a6468b4164d | 248 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 249 | en = recvAckOrNck(); |
| sunifu | 0:5a6468b4164d | 250 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 251 | return en; |
| sunifu | 0:5a6468b4164d | 252 | } |
| sunifu | 0:5a6468b4164d | 253 | |
| sunifu | 0:5a6468b4164d | 254 | /* |
| sunifu | 0:5a6468b4164d | 255 | * Data : snapshot picture |
| sunifu | 0:5a6468b4164d | 256 | */ |
| sunifu | 0:5a6468b4164d | 257 | uint32_t length = 0; |
| sunifu | 0:5a6468b4164d | 258 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 259 | en = recvData(&length); |
| sunifu | 0:5a6468b4164d | 260 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 261 | return en; |
| sunifu | 0:5a6468b4164d | 262 | } |
| sunifu | 0:5a6468b4164d | 263 | en = sendAck(0x00, 0); |
| sunifu | 0:5a6468b4164d | 264 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 265 | return en; |
| sunifu | 0:5a6468b4164d | 266 | } |
| sunifu | 0:5a6468b4164d | 267 | |
| sunifu | 0:5a6468b4164d | 268 | char databuf[packageSize - 6]; |
| sunifu | 0:5a6468b4164d | 269 | uint16_t pkg_total = length / (packageSize - 6); |
| sunifu | 0:5a6468b4164d | 270 | for (int i = 0; i <= (int)pkg_total; i++) { |
| sunifu | 0:5a6468b4164d | 271 | uint16_t checksum = 0; |
| sunifu | 0:5a6468b4164d | 272 | // ID. |
| sunifu | 0:5a6468b4164d | 273 | char idbuf[2]; |
| sunifu | 0:5a6468b4164d | 274 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 275 | if (!RECVFUNC(idbuf, sizeof(idbuf))) { |
| sunifu | 0:5a6468b4164d | 276 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 277 | } |
| sunifu | 0:5a6468b4164d | 278 | checksum += idbuf[0]; |
| sunifu | 0:5a6468b4164d | 279 | checksum += idbuf[1]; |
| sunifu | 0:5a6468b4164d | 280 | uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0); |
| sunifu | 0:5a6468b4164d | 281 | if (id != i) { |
| sunifu | 0:5a6468b4164d | 282 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 283 | } |
| sunifu | 0:5a6468b4164d | 284 | |
| sunifu | 0:5a6468b4164d | 285 | // Size of the data. |
| sunifu | 0:5a6468b4164d | 286 | char dsbuf[2]; |
| sunifu | 0:5a6468b4164d | 287 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 288 | if (!RECVFUNC(dsbuf, sizeof(dsbuf))) { |
| sunifu | 0:5a6468b4164d | 289 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 290 | } |
| sunifu | 0:5a6468b4164d | 291 | |
| sunifu | 0:5a6468b4164d | 292 | // Received the data. |
| sunifu | 0:5a6468b4164d | 293 | checksum += dsbuf[0]; |
| sunifu | 0:5a6468b4164d | 294 | checksum += dsbuf[1]; |
| sunifu | 0:5a6468b4164d | 295 | uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0); |
| sunifu | 0:5a6468b4164d | 296 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 297 | if (!RECVFUNC(&databuf[0], ds)) { |
| sunifu | 0:5a6468b4164d | 298 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 299 | } |
| sunifu | 0:5a6468b4164d | 300 | for (int j = 0; j < ds; j++) { |
| sunifu | 0:5a6468b4164d | 301 | checksum += databuf[j]; |
| sunifu | 0:5a6468b4164d | 302 | } |
| sunifu | 0:5a6468b4164d | 303 | |
| sunifu | 0:5a6468b4164d | 304 | // Verify code. |
| sunifu | 0:5a6468b4164d | 305 | char vcbuf[2]; |
| sunifu | 0:5a6468b4164d | 306 | WAITFUNC(); |
| sunifu | 0:5a6468b4164d | 307 | if (!RECVFUNC(vcbuf, sizeof(vcbuf))) { |
| sunifu | 0:5a6468b4164d | 308 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 309 | } |
| sunifu | 0:5a6468b4164d | 310 | uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0); |
| sunifu | 0:5a6468b4164d | 311 | if (vc != (checksum & 0xff)) { |
| sunifu | 0:5a6468b4164d | 312 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 313 | } |
| sunifu | 0:5a6468b4164d | 314 | |
| sunifu | 0:5a6468b4164d | 315 | /* |
| sunifu | 0:5a6468b4164d | 316 | * Call a call back function. |
| sunifu | 0:5a6468b4164d | 317 | * You can block this function while working. |
| sunifu | 0:5a6468b4164d | 318 | */ |
| hepta2ume | 1:d16a4c3a272a | 319 | size_t siz = ds; |
| hepta2ume | 1:d16a4c3a272a | 320 | for (int ii = 0; ii < (int)siz; ii++) { |
| hepta2ume | 1:d16a4c3a272a | 321 | fprintf(fp_jpeg, "%02X ", databuf[ii]); |
| hepta2ume | 1:d16a4c3a272a | 322 | if(++num%16==0) fprintf(fp_jpeg,"\r\n"); |
| hepta2ume | 1:d16a4c3a272a | 323 | } |
| sunifu | 0:5a6468b4164d | 324 | /* |
| sunifu | 0:5a6468b4164d | 325 | * We should wait for camera working before reply a ACK. |
| sunifu | 0:5a6468b4164d | 326 | */ |
| sunifu | 0:5a6468b4164d | 327 | wait_ms(100); |
| sunifu | 0:5a6468b4164d | 328 | en = sendAck(0x00, 1 + i); |
| sunifu | 0:5a6468b4164d | 329 | if (NoError != en) { |
| sunifu | 0:5a6468b4164d | 330 | return en; |
| sunifu | 0:5a6468b4164d | 331 | } |
| sunifu | 0:5a6468b4164d | 332 | } |
| sunifu | 0:5a6468b4164d | 333 | |
| sunifu | 0:5a6468b4164d | 334 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 335 | } |
| sunifu | 0:5a6468b4164d | 336 | |
| hepta2ume | 2:686c7043f5f4 | 337 | HeptaSerial::ErrorNumber HeptaSerial::sendInitial(Baud baud, JpegResolution jr) |
| hepta2ume | 1:d16a4c3a272a | 338 | { |
| hepta2ume | 1:d16a4c3a272a | 339 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 340 | |
| sunifu | 0:5a6468b4164d | 341 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 342 | send[1] = 0x01; |
| sunifu | 0:5a6468b4164d | 343 | send[2] = (char)baud; |
| sunifu | 0:5a6468b4164d | 344 | send[3] = 0x07; |
| sunifu | 0:5a6468b4164d | 345 | send[4] = 0x00; |
| sunifu | 0:5a6468b4164d | 346 | send[5] = (char)jr; |
| sunifu | 0:5a6468b4164d | 347 | |
| sunifu | 0:5a6468b4164d | 348 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 349 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 350 | } |
| sunifu | 0:5a6468b4164d | 351 | |
| sunifu | 0:5a6468b4164d | 352 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 353 | } |
| sunifu | 0:5a6468b4164d | 354 | |
| hepta2ume | 2:686c7043f5f4 | 355 | HeptaSerial::ErrorNumber HeptaSerial::sendGetPicture(void) |
| hepta2ume | 1:d16a4c3a272a | 356 | { |
| sunifu | 0:5a6468b4164d | 357 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 358 | |
| sunifu | 0:5a6468b4164d | 359 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 360 | send[1] = 0x04; |
| sunifu | 0:5a6468b4164d | 361 | send[2] = 0x01; |
| sunifu | 0:5a6468b4164d | 362 | send[3] = 0x00; |
| sunifu | 0:5a6468b4164d | 363 | send[4] = 0x00; |
| sunifu | 0:5a6468b4164d | 364 | send[5] = 0x00; |
| sunifu | 0:5a6468b4164d | 365 | |
| sunifu | 0:5a6468b4164d | 366 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 367 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 368 | } |
| sunifu | 0:5a6468b4164d | 369 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 370 | } |
| sunifu | 0:5a6468b4164d | 371 | |
| hepta2ume | 2:686c7043f5f4 | 372 | HeptaSerial::ErrorNumber HeptaSerial::sendSnapshot(void) |
| hepta2ume | 1:d16a4c3a272a | 373 | { |
| sunifu | 0:5a6468b4164d | 374 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 375 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 376 | send[1] = 0x05; |
| sunifu | 0:5a6468b4164d | 377 | send[2] = 0x00; |
| sunifu | 0:5a6468b4164d | 378 | send[3] = 0x00; |
| sunifu | 0:5a6468b4164d | 379 | send[4] = 0x00; |
| sunifu | 0:5a6468b4164d | 380 | send[5] = 0x00; |
| sunifu | 0:5a6468b4164d | 381 | |
| sunifu | 0:5a6468b4164d | 382 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 383 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 384 | } |
| sunifu | 0:5a6468b4164d | 385 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 386 | } |
| sunifu | 0:5a6468b4164d | 387 | |
| hepta2ume | 2:686c7043f5f4 | 388 | HeptaSerial::ErrorNumber HeptaSerial::sendSetPackageSize(uint16_t packageSize) |
| hepta2ume | 1:d16a4c3a272a | 389 | { |
| sunifu | 0:5a6468b4164d | 390 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 391 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 392 | send[1] = 0x06; |
| sunifu | 0:5a6468b4164d | 393 | send[2] = 0x08; |
| sunifu | 0:5a6468b4164d | 394 | send[3] = (packageSize >> 0) & 0xff; |
| sunifu | 0:5a6468b4164d | 395 | send[4] = (packageSize >> 8) & 0xff; |
| sunifu | 0:5a6468b4164d | 396 | send[5] = 0x00; |
| sunifu | 0:5a6468b4164d | 397 | |
| sunifu | 0:5a6468b4164d | 398 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 399 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 400 | } |
| sunifu | 0:5a6468b4164d | 401 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 402 | } |
| sunifu | 0:5a6468b4164d | 403 | |
| hepta2ume | 2:686c7043f5f4 | 404 | void HeptaSerial::setmbedBaud(Baud baud) |
| hepta2ume | 1:d16a4c3a272a | 405 | { |
| hepta2ume | 1:d16a4c3a272a | 406 | switch((int)baud) { |
| hepta2ume | 1:d16a4c3a272a | 407 | case 0x02: |
| hepta2ume | 1:d16a4c3a272a | 408 | serial._baud(460800); |
| hepta2ume | 1:d16a4c3a272a | 409 | break; |
| hepta2ume | 1:d16a4c3a272a | 410 | case 0x03: |
| hepta2ume | 1:d16a4c3a272a | 411 | serial._baud(230400); |
| hepta2ume | 1:d16a4c3a272a | 412 | break; |
| hepta2ume | 1:d16a4c3a272a | 413 | case 0x04: |
| hepta2ume | 1:d16a4c3a272a | 414 | serial._baud(115200); |
| hepta2ume | 1:d16a4c3a272a | 415 | break; |
| hepta2ume | 1:d16a4c3a272a | 416 | case 0x05: |
| hepta2ume | 1:d16a4c3a272a | 417 | serial._baud(57600); |
| hepta2ume | 1:d16a4c3a272a | 418 | break; |
| hepta2ume | 1:d16a4c3a272a | 419 | case 0x06: |
| hepta2ume | 1:d16a4c3a272a | 420 | serial._baud((int)28800); |
| hepta2ume | 1:d16a4c3a272a | 421 | break; |
| hepta2ume | 1:d16a4c3a272a | 422 | case 0x07: |
| hepta2ume | 1:d16a4c3a272a | 423 | serial._baud(14400); |
| hepta2ume | 1:d16a4c3a272a | 424 | break; |
| hepta2ume | 1:d16a4c3a272a | 425 | default: |
| hepta2ume | 1:d16a4c3a272a | 426 | serial._baud(14400); |
| sunifu | 0:5a6468b4164d | 427 | } |
| sunifu | 0:5a6468b4164d | 428 | } |
| sunifu | 0:5a6468b4164d | 429 | |
| sunifu | 0:5a6468b4164d | 430 | |
| hepta2ume | 2:686c7043f5f4 | 431 | HeptaSerial::ErrorNumber HeptaSerial::sendReset(ResetType specialReset) |
| hepta2ume | 1:d16a4c3a272a | 432 | { |
| sunifu | 0:5a6468b4164d | 433 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 434 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 435 | send[1] = 0x08; |
| sunifu | 0:5a6468b4164d | 436 | send[2] = 0x00; |
| sunifu | 0:5a6468b4164d | 437 | send[3] = 0x00; |
| sunifu | 0:5a6468b4164d | 438 | send[4] = 0x00; |
| sunifu | 0:5a6468b4164d | 439 | send[5] = specialReset; |
| sunifu | 0:5a6468b4164d | 440 | /* |
| sunifu | 0:5a6468b4164d | 441 | * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately. |
| sunifu | 0:5a6468b4164d | 442 | */ |
| sunifu | 0:5a6468b4164d | 443 | |
| sunifu | 0:5a6468b4164d | 444 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 445 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 446 | } |
| sunifu | 0:5a6468b4164d | 447 | |
| sunifu | 0:5a6468b4164d | 448 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 449 | } |
| sunifu | 0:5a6468b4164d | 450 | |
| sunifu | 0:5a6468b4164d | 451 | |
| hepta2ume | 2:686c7043f5f4 | 452 | HeptaSerial::ErrorNumber HeptaSerial::recvData(uint32_t *length) |
| hepta2ume | 1:d16a4c3a272a | 453 | { |
| sunifu | 0:5a6468b4164d | 454 | char recv[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 455 | if (!RECVFUNC(recv, sizeof(recv))) { |
| sunifu | 0:5a6468b4164d | 456 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 457 | } |
| sunifu | 0:5a6468b4164d | 458 | if ((0xAA != recv[0]) || (0x0A != recv[1])) { |
| sunifu | 0:5a6468b4164d | 459 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 460 | } |
| sunifu | 0:5a6468b4164d | 461 | recv[2] = (char)0x01; |
| sunifu | 0:5a6468b4164d | 462 | *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0); |
| sunifu | 0:5a6468b4164d | 463 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 464 | } |
| sunifu | 0:5a6468b4164d | 465 | |
| hepta2ume | 2:686c7043f5f4 | 466 | HeptaSerial::ErrorNumber HeptaSerial::sendSync() |
| hepta2ume | 1:d16a4c3a272a | 467 | { |
| sunifu | 0:5a6468b4164d | 468 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 469 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 470 | send[1] = 0x0D; |
| sunifu | 0:5a6468b4164d | 471 | send[2] = 0x00; |
| sunifu | 0:5a6468b4164d | 472 | send[3] = 0x00; |
| sunifu | 0:5a6468b4164d | 473 | send[4] = 0x00; |
| sunifu | 0:5a6468b4164d | 474 | send[5] = 0x00; |
| hepta2ume | 1:d16a4c3a272a | 475 | |
| hepta2ume | 1:d16a4c3a272a | 476 | |
| sunifu | 0:5a6468b4164d | 477 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 478 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 479 | } |
| sunifu | 0:5a6468b4164d | 480 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 481 | } |
| sunifu | 0:5a6468b4164d | 482 | |
| hepta2ume | 2:686c7043f5f4 | 483 | HeptaSerial::ErrorNumber HeptaSerial::recvSync() |
| hepta2ume | 1:d16a4c3a272a | 484 | { |
| sunifu | 0:5a6468b4164d | 485 | char recv[COMMAND_LENGTH]; |
| hepta2ume | 1:d16a4c3a272a | 486 | |
| sunifu | 0:5a6468b4164d | 487 | if (!RECVFUNC(recv, sizeof(recv))) { |
| sunifu | 0:5a6468b4164d | 488 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 489 | } |
| sunifu | 0:5a6468b4164d | 490 | if ((0xAA != recv[0]) || (0x0D != recv[1])) { |
| sunifu | 0:5a6468b4164d | 491 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 492 | } |
| sunifu | 0:5a6468b4164d | 493 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 494 | } |
| sunifu | 0:5a6468b4164d | 495 | |
| sunifu | 0:5a6468b4164d | 496 | /** |
| sunifu | 0:5a6468b4164d | 497 | * Send ACK. |
| sunifu | 0:5a6468b4164d | 498 | * |
| sunifu | 0:5a6468b4164d | 499 | * @param commandId The command with that ID is acknowledged by this command. |
| sunifu | 0:5a6468b4164d | 500 | * @param packageId For acknowledging Data command, these two bytes represent the requested package ID. While for acknowledging other commands, these two bytes are set to 00h. |
| sunifu | 0:5a6468b4164d | 501 | */ |
| hepta2ume | 2:686c7043f5f4 | 502 | HeptaSerial::ErrorNumber HeptaSerial::sendAck(uint8_t commandId, uint16_t packageId) |
| hepta2ume | 1:d16a4c3a272a | 503 | { |
| sunifu | 0:5a6468b4164d | 504 | char send[COMMAND_LENGTH]; |
| sunifu | 0:5a6468b4164d | 505 | |
| sunifu | 0:5a6468b4164d | 506 | send[0] = 0xAA; |
| sunifu | 0:5a6468b4164d | 507 | send[1] = 0x0E; |
| sunifu | 0:5a6468b4164d | 508 | send[2] = commandId; |
| sunifu | 0:5a6468b4164d | 509 | send[3] = 0x00; // ACK counter is not used. |
| sunifu | 0:5a6468b4164d | 510 | send[4] = (packageId >> 0) & 0xff; |
| sunifu | 0:5a6468b4164d | 511 | send[5] = (packageId >> 8) & 0xff; |
| sunifu | 0:5a6468b4164d | 512 | if (!SENDFUNC(send, sizeof(send))) { |
| sunifu | 0:5a6468b4164d | 513 | return (ErrorNumber)SendRegisterTimeout; |
| sunifu | 0:5a6468b4164d | 514 | } |
| sunifu | 0:5a6468b4164d | 515 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 516 | } |
| sunifu | 0:5a6468b4164d | 517 | |
| sunifu | 0:5a6468b4164d | 518 | /** |
| sunifu | 0:5a6468b4164d | 519 | * Receive ACK or NCK. |
| sunifu | 0:5a6468b4164d | 520 | * |
| sunifu | 0:5a6468b4164d | 521 | * @return Error number. |
| sunifu | 0:5a6468b4164d | 522 | */ |
| hepta2ume | 2:686c7043f5f4 | 523 | HeptaSerial::ErrorNumber HeptaSerial::recvAckOrNck() |
| hepta2ume | 1:d16a4c3a272a | 524 | { |
| sunifu | 0:5a6468b4164d | 525 | char recv[COMMAND_LENGTH]; |
| hepta2ume | 1:d16a4c3a272a | 526 | |
| sunifu | 0:5a6468b4164d | 527 | if (!RECVFUNC(recv, sizeof(recv))) { |
| sunifu | 0:5a6468b4164d | 528 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 529 | } |
| sunifu | 0:5a6468b4164d | 530 | if ((0xAA == recv[0]) && (0x0E == recv[1])) { |
| sunifu | 0:5a6468b4164d | 531 | return (ErrorNumber)NoError; |
| sunifu | 0:5a6468b4164d | 532 | } |
| sunifu | 0:5a6468b4164d | 533 | if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) { |
| sunifu | 0:5a6468b4164d | 534 | return (ErrorNumber)NoError; |
| hepta2ume | 1:d16a4c3a272a | 535 | } |
| sunifu | 0:5a6468b4164d | 536 | if ((0xAA == recv[0]) && (0x0F == recv[1])) { |
| sunifu | 0:5a6468b4164d | 537 | return (ErrorNumber)recv[4]; |
| sunifu | 0:5a6468b4164d | 538 | } |
| sunifu | 0:5a6468b4164d | 539 | return (ErrorNumber)UnexpectedReply; |
| sunifu | 0:5a6468b4164d | 540 | } |
| sunifu | 0:5a6468b4164d | 541 | |
| sunifu | 0:5a6468b4164d | 542 | /** |
| sunifu | 0:5a6468b4164d | 543 | * Send bytes to camera module. |
| sunifu | 0:5a6468b4164d | 544 | * |
| sunifu | 0:5a6468b4164d | 545 | * @param buf Pointer to the data buffer. |
| sunifu | 0:5a6468b4164d | 546 | * @param len Length of the data buffer. |
| sunifu | 0:5a6468b4164d | 547 | * |
| sunifu | 0:5a6468b4164d | 548 | * @return True if the data sended. |
| sunifu | 0:5a6468b4164d | 549 | */ |
| hepta2ume | 2:686c7043f5f4 | 550 | bool HeptaSerial::sendBytes(char *buf, size_t len, int timeout_us) |
| hepta2ume | 1:d16a4c3a272a | 551 | { |
| sunifu | 0:5a6468b4164d | 552 | for (uint32_t i = 0; i < (uint32_t)len; i++) { |
| sunifu | 0:5a6468b4164d | 553 | int cnt = 0; |
| sunifu | 0:5a6468b4164d | 554 | while (!serial.writeable()) { |
| sunifu | 0:5a6468b4164d | 555 | wait_us(1); |
| sunifu | 0:5a6468b4164d | 556 | cnt++; |
| sunifu | 0:5a6468b4164d | 557 | if (timeout_us < cnt) { |
| sunifu | 0:5a6468b4164d | 558 | return false; |
| sunifu | 0:5a6468b4164d | 559 | } |
| sunifu | 0:5a6468b4164d | 560 | } |
| sunifu | 0:5a6468b4164d | 561 | serial.putc(buf[i]); |
| sunifu | 0:5a6468b4164d | 562 | } |
| sunifu | 0:5a6468b4164d | 563 | return true; |
| sunifu | 0:5a6468b4164d | 564 | } |
| sunifu | 0:5a6468b4164d | 565 | |
| sunifu | 0:5a6468b4164d | 566 | /** |
| sunifu | 0:5a6468b4164d | 567 | * Receive bytes from camera module. |
| sunifu | 0:5a6468b4164d | 568 | * |
| sunifu | 0:5a6468b4164d | 569 | * @param buf Pointer to the data buffer. |
| sunifu | 0:5a6468b4164d | 570 | * @param len Length of the data buffer. |
| sunifu | 0:5a6468b4164d | 571 | * |
| sunifu | 0:5a6468b4164d | 572 | * @return True if the data received. |
| sunifu | 0:5a6468b4164d | 573 | */ |
| hepta2ume | 2:686c7043f5f4 | 574 | bool HeptaSerial::recvBytes(char *buf, size_t len, int timeout_us) |
| hepta2ume | 1:d16a4c3a272a | 575 | { |
| sunifu | 0:5a6468b4164d | 576 | for (uint32_t i = 0; i < (uint32_t)len; i++) { |
| sunifu | 0:5a6468b4164d | 577 | int cnt = 0; |
| sunifu | 0:5a6468b4164d | 578 | while (!serial.readable()) { |
| sunifu | 0:5a6468b4164d | 579 | wait_us(1); |
| sunifu | 0:5a6468b4164d | 580 | cnt++; |
| sunifu | 0:5a6468b4164d | 581 | if (timeout_us < cnt) { |
| sunifu | 0:5a6468b4164d | 582 | return false; |
| sunifu | 0:5a6468b4164d | 583 | } |
| sunifu | 0:5a6468b4164d | 584 | } |
| sunifu | 0:5a6468b4164d | 585 | buf[i] = serial.getc(); |
| sunifu | 0:5a6468b4164d | 586 | } |
| sunifu | 0:5a6468b4164d | 587 | return true; |
| sunifu | 0:5a6468b4164d | 588 | } |
| sunifu | 0:5a6468b4164d | 589 | |
| sunifu | 0:5a6468b4164d | 590 | /** |
| sunifu | 0:5a6468b4164d | 591 | * Wait received. |
| sunifu | 0:5a6468b4164d | 592 | * |
| sunifu | 0:5a6468b4164d | 593 | * @return True if the data received. |
| sunifu | 0:5a6468b4164d | 594 | */ |
| hepta2ume | 2:686c7043f5f4 | 595 | bool HeptaSerial::waitRecv() |
| hepta2ume | 1:d16a4c3a272a | 596 | { |
| sunifu | 0:5a6468b4164d | 597 | while (!serial.readable()) { |
| sunifu | 0:5a6468b4164d | 598 | } |
| sunifu | 0:5a6468b4164d | 599 | return true; |
| sunifu | 0:5a6468b4164d | 600 | } |
| sunifu | 0:5a6468b4164d | 601 | |
| sunifu | 0:5a6468b4164d | 602 | /** |
| sunifu | 0:5a6468b4164d | 603 | * Wait idle state. |
| sunifu | 0:5a6468b4164d | 604 | */ |
| hepta2ume | 2:686c7043f5f4 | 605 | bool HeptaSerial::waitIdle() |
| hepta2ume | 1:d16a4c3a272a | 606 | { |
| sunifu | 0:5a6468b4164d | 607 | while (serial.readable()) { |
| sunifu | 0:5a6468b4164d | 608 | serial.getc(); |
| sunifu | 0:5a6468b4164d | 609 | } |
| sunifu | 0:5a6468b4164d | 610 | return true; |
| sunifu | 0:5a6468b4164d | 611 | } |
| hepta2ume | 1:d16a4c3a272a | 612 | |
| hepta2ume | 2:686c7043f5f4 | 613 | void HeptaSerial::Sync(void) |
| hepta2ume | 1:d16a4c3a272a | 614 | { |
| hepta2ume | 5:50a03fcd9cc0 | 615 | serial.setTimeout(1); |
| hepta2ume | 2:686c7043f5f4 | 616 | HeptaSerial::ErrorNumber err = HeptaSerial::NoError; |
| hepta2ume | 5:50a03fcd9cc0 | 617 | |
| hepta2ume | 5:50a03fcd9cc0 | 618 | err = sync(); |
| hepta2ume | 5:50a03fcd9cc0 | 619 | int count=0; |
| hepta2ume | 5:50a03fcd9cc0 | 620 | while(err) { |
| hepta2ume | 6:c11f48bbb567 | 621 | printf("count = %d\r\n",count); |
| hepta2ume | 5:50a03fcd9cc0 | 622 | switch(count) { |
| hepta2ume | 5:50a03fcd9cc0 | 623 | case 0: |
| hepta2ume | 5:50a03fcd9cc0 | 624 | setmbedBaud(HeptaSerial::Baud14400); |
| hepta2ume | 5:50a03fcd9cc0 | 625 | break; |
| hepta2ume | 5:50a03fcd9cc0 | 626 | case 1: |
| hepta2ume | 5:50a03fcd9cc0 | 627 | setmbedBaud(HeptaSerial::Baud115200); |
| hepta2ume | 5:50a03fcd9cc0 | 628 | break; |
| hepta2ume | 5:50a03fcd9cc0 | 629 | default: |
| hepta2ume | 5:50a03fcd9cc0 | 630 | count=0; |
| hepta2ume | 5:50a03fcd9cc0 | 631 | } |
| hepta2ume | 5:50a03fcd9cc0 | 632 | count++; |
| hepta2ume | 5:50a03fcd9cc0 | 633 | err = sync(); |
| hepta2ume | 5:50a03fcd9cc0 | 634 | printf("init err1=%d\r\n", err); |
| hepta2ume | 6:c11f48bbb567 | 635 | if(!err) { |
| hepta2ume | 6:c11f48bbb567 | 636 | printf("to 115200\r\n"); |
| hepta2ume | 6:c11f48bbb567 | 637 | init(HeptaSerial::Baud115200, HeptaSerial::JpegResolution320x240); |
| hepta2ume | 6:c11f48bbb567 | 638 | printf("init err2=%d\r\n", err); |
| hepta2ume | 6:c11f48bbb567 | 639 | setmbedBaud(HeptaSerial::Baud115200); |
| hepta2ume | 6:c11f48bbb567 | 640 | printf("init err3=%d\r\n", err); |
| hepta2ume | 6:c11f48bbb567 | 641 | } |
| hepta2ume | 6:c11f48bbb567 | 642 | } |
| hepta2ume | 1:d16a4c3a272a | 643 | } |
| hepta2ume | 1:d16a4c3a272a | 644 | |
| hepta2ume | 3:12659e671bad | 645 | void HeptaSerial::test_jpeg_snapshot_picture(int CAPTURE_FRAMES) |
| hepta2ume | 1:d16a4c3a272a | 646 | { |
| hepta2ume | 2:686c7043f5f4 | 647 | HeptaSerial::ErrorNumber err = HeptaSerial::NoError; |
| hepta2ume | 1:d16a4c3a272a | 648 | for (int i = 0; i < CAPTURE_FRAMES; i++) { |
| hepta2ume | 1:d16a4c3a272a | 649 | char fname[64]; |
| hepta2ume | 5:50a03fcd9cc0 | 650 | snprintf(fname, sizeof(fname), "/fs/test.jpg"); |
| hepta2ume | 1:d16a4c3a272a | 651 | fp_jpeg = fopen(fname, "w"); |
| hepta2ume | 1:d16a4c3a272a | 652 | |
| hepta2ume | 1:d16a4c3a272a | 653 | err = getJpegSnapshotPicture(); |
| hepta2ume | 1:d16a4c3a272a | 654 | |
| hepta2ume | 2:686c7043f5f4 | 655 | if (HeptaSerial::NoError == err) { |
| hepta2ume | 2:686c7043f5f4 | 656 | printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n"); |
| hepta2ume | 1:d16a4c3a272a | 657 | } else { |
| hepta2ume | 2:686c7043f5f4 | 658 | printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err); |
| hepta2ume | 1:d16a4c3a272a | 659 | } |
| hepta2ume | 1:d16a4c3a272a | 660 | fclose(fp_jpeg); |
| hepta2ume | 1:d16a4c3a272a | 661 | } |
| hepta2ume | 1:d16a4c3a272a | 662 | } |
| hepta2ume | 1:d16a4c3a272a | 663 | |
| hepta2ume | 3:12659e671bad | 664 | void HeptaSerial::test_jpeg_snapshot_data(int CAPTURE_FRAMES) |
| hepta2ume | 1:d16a4c3a272a | 665 | { |
| hepta2ume | 2:686c7043f5f4 | 666 | HeptaSerial::ErrorNumber err = HeptaSerial::NoError; |
| hepta2ume | 1:d16a4c3a272a | 667 | for (int i = 0; i < CAPTURE_FRAMES; i++) { |
| hepta2ume | 1:d16a4c3a272a | 668 | char fname[64]; |
| hepta2ume | 5:50a03fcd9cc0 | 669 | snprintf(fname, sizeof(fname), "/fs/test.txt"); |
| hepta2ume | 1:d16a4c3a272a | 670 | fp_jpeg = fopen(fname, "w"); |
| hepta2ume | 1:d16a4c3a272a | 671 | |
| hepta2ume | 1:d16a4c3a272a | 672 | err = getJpegSnapshotPicture_data(); |
| hepta2ume | 1:d16a4c3a272a | 673 | |
| hepta2ume | 2:686c7043f5f4 | 674 | if (HeptaSerial::NoError == err) { |
| hepta2ume | 2:686c7043f5f4 | 675 | printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n"); |
| hepta2ume | 1:d16a4c3a272a | 676 | } else { |
| hepta2ume | 2:686c7043f5f4 | 677 | printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err); |
| hepta2ume | 1:d16a4c3a272a | 678 | } |
| hepta2ume | 1:d16a4c3a272a | 679 | fclose(fp_jpeg); |
| hepta2ume | 1:d16a4c3a272a | 680 | } |
| hepta2ume | 1:d16a4c3a272a | 681 | } |
| hepta2ume | 1:d16a4c3a272a | 682 | |
| hepta2ume | 2:686c7043f5f4 | 683 | void HeptaSerial::initialize(Baud baud,JpegResolution jr) |
| hepta2ume | 1:d16a4c3a272a | 684 | { |
| hepta2ume | 2:686c7043f5f4 | 685 | HeptaSerial::ErrorNumber err = HeptaSerial::NoError;//ErrorNumber define |
| hepta2ume | 1:d16a4c3a272a | 686 | err = init(baud, jr);// |
| hepta2ume | 2:686c7043f5f4 | 687 | if (HeptaSerial::NoError == err) { |
| hepta2ume | 2:686c7043f5f4 | 688 | printf("[ OK ] : Camera::init\r\n"); |
| hepta2ume | 5:50a03fcd9cc0 | 689 | setmbedBaud(baud); |
| hepta2ume | 1:d16a4c3a272a | 690 | } else { |
| hepta2ume | 2:686c7043f5f4 | 691 | printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err); |
| hepta2ume | 1:d16a4c3a272a | 692 | } |
| hepta2ume | 1:d16a4c3a272a | 693 | |
| hepta2ume | 4:57c7f33a3621 | 694 | } |
| hepta2ume | 4:57c7f33a3621 | 695 | |
| hepta2ume | 4:57c7f33a3621 | 696 | //*********************serial*********************// |
| hepta2ume | 6:c11f48bbb567 | 697 | void HeptaSerial::gps_setting(void) |
| hepta2ume | 6:c11f48bbb567 | 698 | { |
| hepta2ume | 6:c11f48bbb567 | 699 | serial._baud(9600); |
| hepta2ume | 6:c11f48bbb567 | 700 | serial.setTimeout(9999); |
| hepta2ume | 6:c11f48bbb567 | 701 | } |
| hepta2ume | 6:c11f48bbb567 | 702 | |
| hepta2ume | 6:c11f48bbb567 | 703 | |
| hepta2ume | 4:57c7f33a3621 | 704 | char HeptaSerial::getc() |
| hepta2ume | 4:57c7f33a3621 | 705 | { |
| hepta2ume | 6:c11f48bbb567 | 706 | gps_setting(); |
| hepta2ume | 4:57c7f33a3621 | 707 | c = serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 708 | return c; |
| hepta2ume | 4:57c7f33a3621 | 709 | } |
| hepta2ume | 4:57c7f33a3621 | 710 | int HeptaSerial::readable() |
| hepta2ume | 4:57c7f33a3621 | 711 | { |
| hepta2ume | 6:c11f48bbb567 | 712 | gps_setting(); |
| hepta2ume | 4:57c7f33a3621 | 713 | i = serial.readable(); |
| hepta2ume | 4:57c7f33a3621 | 714 | return i; |
| hepta2ume | 4:57c7f33a3621 | 715 | } |
| hepta2ume | 4:57c7f33a3621 | 716 | void HeptaSerial::flushSerialBuffer(void) |
| hepta2ume | 4:57c7f33a3621 | 717 | { |
| hepta2ume | 6:c11f48bbb567 | 718 | gps_setting(); |
| hepta2ume | 4:57c7f33a3621 | 719 | ite = 0; |
| hepta2ume | 6:c11f48bbb567 | 720 | while (serial.readable()) { |
| hepta2ume | 4:57c7f33a3621 | 721 | serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 722 | ite++; |
| hepta2ume | 6:c11f48bbb567 | 723 | if(ite==100) { |
| hepta2ume | 6:c11f48bbb567 | 724 | break; |
| hepta2ume | 6:c11f48bbb567 | 725 | }; |
| hepta2ume | 4:57c7f33a3621 | 726 | } |
| hepta2ume | 4:57c7f33a3621 | 727 | return; |
| hepta2ume | 4:57c7f33a3621 | 728 | } |
| hepta2ume | 4:57c7f33a3621 | 729 | void HeptaSerial::gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *serial_check) |
| hepta2ume | 4:57c7f33a3621 | 730 | { |
| hepta2ume | 6:c11f48bbb567 | 731 | gps_setting(); |
| hepta2ume | 4:57c7f33a3621 | 732 | int ite = 0; |
| hepta2ume | 6:c11f48bbb567 | 733 | while(serial.getc()!='$') { |
| hepta2ume | 4:57c7f33a3621 | 734 | ite++; |
| hepta2ume | 4:57c7f33a3621 | 735 | if(ite==10000) break; |
| hepta2ume | 4:57c7f33a3621 | 736 | } |
| hepta2ume | 6:c11f48bbb567 | 737 | for(int i=0; i<5; i++) { |
| hepta2ume | 4:57c7f33a3621 | 738 | msg[i] = serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 739 | } |
| hepta2ume | 6:c11f48bbb567 | 740 | if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) { |
| hepta2ume | 6:c11f48bbb567 | 741 | for(int j=0; j<6; j++) { |
| hepta2ume | 6:c11f48bbb567 | 742 | if(j==0) { |
| hepta2ume | 6:c11f48bbb567 | 743 | for(int i=5; i<256; i++) { |
| hepta2ume | 4:57c7f33a3621 | 744 | msg[i] = serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 745 | if(msg[i] == '\r') { |
| hepta2ume | 4:57c7f33a3621 | 746 | msg[i] = 0; |
| hepta2ume | 4:57c7f33a3621 | 747 | break; |
| hepta2ume | 4:57c7f33a3621 | 748 | } |
| hepta2ume | 4:57c7f33a3621 | 749 | } |
| hepta2ume | 6:c11f48bbb567 | 750 | } else { |
| hepta2ume | 6:c11f48bbb567 | 751 | for(int i=0; i<256; i++) { |
| hepta2ume | 4:57c7f33a3621 | 752 | msgd[i] = serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 753 | if(msgd[i] == '\r') { |
| hepta2ume | 4:57c7f33a3621 | 754 | break; |
| hepta2ume | 4:57c7f33a3621 | 755 | } |
| hepta2ume | 4:57c7f33a3621 | 756 | } |
| hepta2ume | 6:c11f48bbb567 | 757 | if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) { |
| hepta2ume | 4:57c7f33a3621 | 758 | break; |
| hepta2ume | 4:57c7f33a3621 | 759 | } |
| hepta2ume | 4:57c7f33a3621 | 760 | } |
| hepta2ume | 4:57c7f33a3621 | 761 | } |
| hepta2ume | 6:c11f48bbb567 | 762 | if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", time, latitude, ns, longitude, ew, quality, stnum, hacu, altitude, aunit) >= 1) { |
| hepta2ume | 4:57c7f33a3621 | 763 | if(!(quality)) { |
| hepta2ume | 4:57c7f33a3621 | 764 | //latitude(unit transformation) |
| hepta2ume | 4:57c7f33a3621 | 765 | *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 766 | //longitude(unit transformation) |
| hepta2ume | 4:57c7f33a3621 | 767 | *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 768 | *serial_check = 0; |
| hepta2ume | 4:57c7f33a3621 | 769 | } else { |
| hepta2ume | 4:57c7f33a3621 | 770 | //latitude(unit transformation) |
| hepta2ume | 4:57c7f33a3621 | 771 | *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 772 | //longitude(unit transformation) |
| hepta2ume | 4:57c7f33a3621 | 773 | *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 774 | *serial_check = 1; |
| hepta2ume | 4:57c7f33a3621 | 775 | } |
| hepta2ume | 6:c11f48bbb567 | 776 | } else { |
| hepta2ume | 4:57c7f33a3621 | 777 | printf("No Data"); |
| hepta2ume | 4:57c7f33a3621 | 778 | *serial_check = 2; |
| hepta2ume | 6:c11f48bbb567 | 779 | } |
| hepta2ume | 6:c11f48bbb567 | 780 | } else { |
| hepta2ume | 4:57c7f33a3621 | 781 | *serial_check = 3; |
| hepta2ume | 4:57c7f33a3621 | 782 | } |
| hepta2ume | 4:57c7f33a3621 | 783 | } |
| hepta2ume | 6:c11f48bbb567 | 784 | void HeptaSerial::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2) |
| hepta2ume | 4:57c7f33a3621 | 785 | { |
| hepta2ume | 6:c11f48bbb567 | 786 | gps_setting(); |
| hepta2ume | 6:c11f48bbb567 | 787 | height_1 = 29.4; |
| hepta2ume | 6:c11f48bbb567 | 788 | char gph1[8]= {0x00},gph2[8]= {0x00},gph3[8]= {0x00},gph4[8]= {0x00},gpt1[8]= {0x00},gpt2[8]= {0x00},gpt3[8]= {0x00},gpt4[8]= {0x00}; |
| hepta2ume | 6:c11f48bbb567 | 789 | char hei1[8]= {0x00},hei2[8]= {0x00}; |
| hepta2ume | 4:57c7f33a3621 | 790 | int i=0,j=0; |
| hepta2ume | 6:c11f48bbb567 | 791 | while (serial.readable()) { |
| hepta2ume | 4:57c7f33a3621 | 792 | serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 793 | } |
| hepta2ume | 6:c11f48bbb567 | 794 | loop: |
| hepta2ume | 6:c11f48bbb567 | 795 | while(serial.getc()!='$') {} |
| hepta2ume | 6:c11f48bbb567 | 796 | for(j=0; j<5; j++) { |
| hepta2ume | 4:57c7f33a3621 | 797 | gps_data[1][j]=serial.getc(); |
| hepta2ume | 4:57c7f33a3621 | 798 | } |
| hepta2ume | 6:c11f48bbb567 | 799 | if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)) { |
| hepta2ume | 6:c11f48bbb567 | 800 | for(j=0; j<1; j++) { |
| hepta2ume | 6:c11f48bbb567 | 801 | if(j==0) { |
| hepta2ume | 4:57c7f33a3621 | 802 | i=0; |
| hepta2ume | 6:c11f48bbb567 | 803 | while((gps_data[j+1][i+5] = serial.getc()) != '\r') { |
| hepta2ume | 4:57c7f33a3621 | 804 | //pc.putc(gps_data[j+1][i+5]); |
| hepta2ume | 4:57c7f33a3621 | 805 | i++; |
| hepta2ume | 4:57c7f33a3621 | 806 | } |
| hepta2ume | 4:57c7f33a3621 | 807 | gps_data[j+1][i+5]='\0'; |
| hepta2ume | 4:57c7f33a3621 | 808 | i=0; |
| hepta2ume | 4:57c7f33a3621 | 809 | //pc.printf("\n\r"); |
| hepta2ume | 6:c11f48bbb567 | 810 | } else { |
| hepta2ume | 6:c11f48bbb567 | 811 | while(serial.getc()!='$') {} |
| hepta2ume | 4:57c7f33a3621 | 812 | i=0; |
| hepta2ume | 6:c11f48bbb567 | 813 | while((gps_data[j+1][i] = serial.getc()) != '\r') { |
| hepta2ume | 4:57c7f33a3621 | 814 | //pc.putc(gps_data[j+1][i]); |
| hepta2ume | 4:57c7f33a3621 | 815 | i++; |
| hepta2ume | 4:57c7f33a3621 | 816 | } |
| hepta2ume | 4:57c7f33a3621 | 817 | gps_data[j+1][i]='\0'; |
| hepta2ume | 4:57c7f33a3621 | 818 | i=0; |
| hepta2ume | 4:57c7f33a3621 | 819 | //pc.printf("\n\r"); |
| hepta2ume | 4:57c7f33a3621 | 820 | } |
| hepta2ume | 4:57c7f33a3621 | 821 | } |
| hepta2ume | 6:c11f48bbb567 | 822 | } else { |
| hepta2ume | 4:57c7f33a3621 | 823 | goto loop; |
| hepta2ume | 4:57c7f33a3621 | 824 | } |
| hepta2ume | 6:c11f48bbb567 | 825 | if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1) { |
| hepta2ume | 4:57c7f33a3621 | 826 | //hokui |
| hepta2ume | 4:57c7f33a3621 | 827 | d_hokui=int(hokui/100); |
| hepta2ume | 4:57c7f33a3621 | 828 | m_hokui=(hokui-d_hokui*100); |
| hepta2ume | 4:57c7f33a3621 | 829 | //m_hokui=(hokui-d_hokui*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 830 | g_hokui=d_hokui+(hokui-d_hokui*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 831 | sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 832 | sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 833 | sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 834 | sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF); |
| hepta2ume | 6:c11f48bbb567 | 835 | |
| hepta2ume | 4:57c7f33a3621 | 836 | //tokei |
| hepta2ume | 4:57c7f33a3621 | 837 | d_tokei=int(tokei/100); |
| hepta2ume | 4:57c7f33a3621 | 838 | m_tokei=(tokei-d_tokei*100); |
| hepta2ume | 4:57c7f33a3621 | 839 | //m_tokei=(tokei-d_tokei*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 840 | g_tokei=d_tokei+(tokei-d_tokei*100)/60; |
| hepta2ume | 4:57c7f33a3621 | 841 | sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 842 | sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 843 | sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF); |
| hepta2ume | 4:57c7f33a3621 | 844 | sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF); |
| hepta2ume | 6:c11f48bbb567 | 845 | |
| hepta2ume | 6:c11f48bbb567 | 846 | m_height = int(height_1); |
| hepta2ume | 6:c11f48bbb567 | 847 | cm_height = int((height_1-m_height)*100 ); |
| hepta2ume | 6:c11f48bbb567 | 848 | //printf("%d\r\n",cm_height); |
| hepta2ume | 6:c11f48bbb567 | 849 | sprintf( hei1, "%02X", (char(m_height)) & 0xFF); |
| hepta2ume | 6:c11f48bbb567 | 850 | sprintf( hei2, "%02X", (char(cm_height)) & 0xFF); |
| hepta2ume | 6:c11f48bbb567 | 851 | |
| hepta2ume | 4:57c7f33a3621 | 852 | lat[0] = gph1[0]; |
| hepta2ume | 4:57c7f33a3621 | 853 | lat[1] = gph1[1]; |
| hepta2ume | 4:57c7f33a3621 | 854 | lat[2] = gph2[0]; |
| hepta2ume | 4:57c7f33a3621 | 855 | lat[3] = gph2[1]; |
| hepta2ume | 4:57c7f33a3621 | 856 | lat[4] = gph3[0]; |
| hepta2ume | 4:57c7f33a3621 | 857 | lat[5] = gph3[1]; |
| hepta2ume | 4:57c7f33a3621 | 858 | lat[6] = gph4[0]; |
| hepta2ume | 4:57c7f33a3621 | 859 | lat[7] = gph4[1]; |
| hepta2ume | 4:57c7f33a3621 | 860 | log[0] = gpt1[0]; |
| hepta2ume | 4:57c7f33a3621 | 861 | log[1] = gpt1[1]; |
| hepta2ume | 4:57c7f33a3621 | 862 | log[2] = gpt2[0]; |
| hepta2ume | 4:57c7f33a3621 | 863 | log[3] = gpt2[1]; |
| hepta2ume | 4:57c7f33a3621 | 864 | log[4] = gpt3[0]; |
| hepta2ume | 4:57c7f33a3621 | 865 | log[5] = gpt3[1]; |
| hepta2ume | 4:57c7f33a3621 | 866 | log[6] = gpt4[0]; |
| hepta2ume | 6:c11f48bbb567 | 867 | log[7] = gpt4[1]; |
| hepta2ume | 6:c11f48bbb567 | 868 | height[0] = hei1[0]; |
| hepta2ume | 6:c11f48bbb567 | 869 | height[1] = hei1[1]; |
| hepta2ume | 6:c11f48bbb567 | 870 | height[2] = hei2[0]; |
| hepta2ume | 6:c11f48bbb567 | 871 | height[3] = hei2[1]; |
| hepta2ume | 4:57c7f33a3621 | 872 | } |
| hepta2ume | 6:c11f48bbb567 | 873 | *dsize1 = 8; |
| hepta2ume | 6:c11f48bbb567 | 874 | *dsize2 = 4; |
| hepta2ume | 1:d16a4c3a272a | 875 | } |