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