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