HEPTA-Sat Training 2017~2018 / HeptaCamera_GPS

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 CLTP 8

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?

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