CLTP 8 / HeptaCamera_GPS

test

Dependents:   HEPTA_assembly template HEPTA_GS

HeptaCamera_GPS.cpp@8:6d9c33df4c09, 2017-08-23 (annotated)

Committer:
umeume
Date:
Wed Aug 23 06:18:42 2017 +0000
Revision:
8:6d9c33df4c09
test

Who changed what in which revision?

UserRevisionLine numberNew 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();
umeume 8:6d9c33df4c09 613 int count=0;
umeume 8:6d9c33df4c09 614 while(err) {
umeume 8:6d9c33df4c09 615 //printf("count = %d\r\n",count);
umeume 8:6d9c33df4c09 616 switch(count) {
umeume 8:6d9c33df4c09 617 case 0:
umeume 8:6d9c33df4c09 618 printf("Connection of camera and mbed at baudrate 14400\r\n");
umeume 8:6d9c33df4c09 619 setmbedBaud(HeptaCamera_GPS::Baud14400);
umeume 8:6d9c33df4c09 620
umeume 8:6d9c33df4c09 621 break;
umeume 8:6d9c33df4c09 622 case 1:
umeume 8:6d9c33df4c09 623 printf("Connection of camera and mbed at baudrate 115200\r\n");
umeume 8:6d9c33df4c09 624 setmbedBaud(HeptaCamera_GPS::Baud115200);
umeume 8:6d9c33df4c09 625 break;
umeume 8:6d9c33df4c09 626 default:
umeume 8:6d9c33df4c09 627 count=0;
umeume 8:6d9c33df4c09 628 }
umeume 8:6d9c33df4c09 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 /* if(!err) {
umeume 8:6d9c33df4c09 636 printf("to 115200\r\n");
umeume 8:6d9c33df4c09 637 init(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
umeume 8:6d9c33df4c09 638 printf("init err2=%d\r\n", err);
umeume 8:6d9c33df4c09 639 setmbedBaud(HeptaCamera_GPS::Baud115200);
umeume 8:6d9c33df4c09 640 printf("init err3=%d\r\n", err);
umeume 8:6d9c33df4c09 641 }*/
umeume 8:6d9c33df4c09 642 }
umeume 8:6d9c33df4c09 643 }
umeume 8:6d9c33df4c09 644
umeume 8:6d9c33df4c09 645 void HeptaCamera_GPS::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
umeume 8:6d9c33df4c09 646 {
umeume 8:6d9c33df4c09 647 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
umeume 8:6d9c33df4c09 648 for (int i = 0; i < CAPTURE_FRAMES; i++) {
umeume 8:6d9c33df4c09 649 char fname[64];
umeume 8:6d9c33df4c09 650 snprintf(fname, sizeof(fname), "/fs/test.jpg");
umeume 8:6d9c33df4c09 651 fp_jpeg = fopen(fname, "w");
umeume 8:6d9c33df4c09 652
umeume 8:6d9c33df4c09 653 err = getJpegSnapshotPicture();
umeume 8:6d9c33df4c09 654
umeume 8:6d9c33df4c09 655 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 656 printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
umeume 8:6d9c33df4c09 657 } else {
umeume 8:6d9c33df4c09 658 printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 659 }
umeume 8:6d9c33df4c09 660 fclose(fp_jpeg);
umeume 8:6d9c33df4c09 661 }
umeume 8:6d9c33df4c09 662 }
umeume 8:6d9c33df4c09 663
umeume 8:6d9c33df4c09 664 void HeptaCamera_GPS::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
umeume 8:6d9c33df4c09 665 {
umeume 8:6d9c33df4c09 666 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
umeume 8:6d9c33df4c09 667 for (int i = 0; i < CAPTURE_FRAMES; i++) {
umeume 8:6d9c33df4c09 668 char fname[64];
umeume 8:6d9c33df4c09 669 snprintf(fname, sizeof(fname), "/fs/test.txt");
umeume 8:6d9c33df4c09 670 fp_jpeg = fopen(fname, "w");
umeume 8:6d9c33df4c09 671
umeume 8:6d9c33df4c09 672 err = getJpegSnapshotPicture_data();
umeume 8:6d9c33df4c09 673
umeume 8:6d9c33df4c09 674 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 675 printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
umeume 8:6d9c33df4c09 676 } else {
umeume 8:6d9c33df4c09 677 printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 678 }
umeume 8:6d9c33df4c09 679 fclose(fp_jpeg);
umeume 8:6d9c33df4c09 680 }
umeume 8:6d9c33df4c09 681 }
umeume 8:6d9c33df4c09 682
umeume 8:6d9c33df4c09 683 void HeptaCamera_GPS::initialize(Baud baud,JpegResolution jr)
umeume 8:6d9c33df4c09 684 {
umeume 8:6d9c33df4c09 685 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;//ErrorNumber define
umeume 8:6d9c33df4c09 686 err = init(baud, jr);//
umeume 8:6d9c33df4c09 687 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 688 printf("[ OK ] : Camera::init\r\n");
umeume 8:6d9c33df4c09 689 setmbedBaud(baud);
umeume 8:6d9c33df4c09 690 } else {
umeume 8:6d9c33df4c09 691 printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 692 }
umeume 8:6d9c33df4c09 693
umeume 8:6d9c33df4c09 694 }
umeume 8:6d9c33df4c09 695
umeume 8:6d9c33df4c09 696 //*********************serial*********************//
umeume 8:6d9c33df4c09 697 void HeptaCamera_GPS::gps_setting(void)
umeume 8:6d9c33df4c09 698 {
umeume 8:6d9c33df4c09 699 CAM_SW = 0;
umeume 8:6d9c33df4c09 700 GPS_SW = 1;
umeume 8:6d9c33df4c09 701 serial._baud(9600);
umeume 8:6d9c33df4c09 702 serial.setTimeout(9999);
umeume 8:6d9c33df4c09 703 }
umeume 8:6d9c33df4c09 704
umeume 8:6d9c33df4c09 705
umeume 8:6d9c33df4c09 706 char HeptaCamera_GPS::getc()
umeume 8:6d9c33df4c09 707 {
umeume 8:6d9c33df4c09 708 //gps_setting();
umeume 8:6d9c33df4c09 709 //flushSerialBuffer();
umeume 8:6d9c33df4c09 710 c = serial.getc();
umeume 8:6d9c33df4c09 711 return c;
umeume 8:6d9c33df4c09 712 }
umeume 8:6d9c33df4c09 713 int HeptaCamera_GPS::readable()
umeume 8:6d9c33df4c09 714 {
umeume 8:6d9c33df4c09 715 //gps_setting();
umeume 8:6d9c33df4c09 716 i = serial.readable();
umeume 8:6d9c33df4c09 717 return i;
umeume 8:6d9c33df4c09 718 }
umeume 8:6d9c33df4c09 719 void HeptaCamera_GPS::flushSerialBuffer(void)
umeume 8:6d9c33df4c09 720 {
umeume 8:6d9c33df4c09 721 //gps_setting();
umeume 8:6d9c33df4c09 722 ite = 0;
umeume 8:6d9c33df4c09 723 while (serial.readable()) {
umeume 8:6d9c33df4c09 724 serial.getc();
umeume 8:6d9c33df4c09 725 ite++;
umeume 8:6d9c33df4c09 726 if(ite==100) {
umeume 8:6d9c33df4c09 727 break;
umeume 8:6d9c33df4c09 728 };
umeume 8:6d9c33df4c09 729 }
umeume 8:6d9c33df4c09 730 return;
umeume 8:6d9c33df4c09 731 }
umeume 8:6d9c33df4c09 732 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 733 {
umeume 8:6d9c33df4c09 734 //flushSerialBuffer();
umeume 8:6d9c33df4c09 735 int ite = 0;
umeume 8:6d9c33df4c09 736 while(serial.getc()!='$') {
umeume 8:6d9c33df4c09 737 ite++;
umeume 8:6d9c33df4c09 738 if(ite==10000) break;
umeume 8:6d9c33df4c09 739 }
umeume 8:6d9c33df4c09 740 for(int i=0; i<5; i++) {
umeume 8:6d9c33df4c09 741 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 742 }
umeume 8:6d9c33df4c09 743 if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
umeume 8:6d9c33df4c09 744 for(int j=0; j<6; j++) {
umeume 8:6d9c33df4c09 745 if(j==0) {
umeume 8:6d9c33df4c09 746 for(int i=5; i<256; i++) {
umeume 8:6d9c33df4c09 747 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 748 if(msg[i] == '\r') {
umeume 8:6d9c33df4c09 749 msg[i] = 0;
umeume 8:6d9c33df4c09 750 break;
umeume 8:6d9c33df4c09 751 }
umeume 8:6d9c33df4c09 752 }
umeume 8:6d9c33df4c09 753 } else {
umeume 8:6d9c33df4c09 754 for(int i=0; i<256; i++) {
umeume 8:6d9c33df4c09 755 msgd[i] = serial.getc();
umeume 8:6d9c33df4c09 756 if(msgd[i] == '\r') {
umeume 8:6d9c33df4c09 757 break;
umeume 8:6d9c33df4c09 758 }
umeume 8:6d9c33df4c09 759 }
umeume 8:6d9c33df4c09 760 if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
umeume 8:6d9c33df4c09 761 break;
umeume 8:6d9c33df4c09 762 }
umeume 8:6d9c33df4c09 763 }
umeume 8:6d9c33df4c09 764 }
umeume 8:6d9c33df4c09 765 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 766 if(!(quality)) {
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 = 0;
umeume 8:6d9c33df4c09 772 } else {
umeume 8:6d9c33df4c09 773 //latitude(unit transformation)
umeume 8:6d9c33df4c09 774 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
umeume 8:6d9c33df4c09 775 //longitude(unit transformation)
umeume 8:6d9c33df4c09 776 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
umeume 8:6d9c33df4c09 777 *serial_check = 1;
umeume 8:6d9c33df4c09 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
umeume 8:6d9c33df4c09 788 void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
umeume 8:6d9c33df4c09 789 {
umeume 8:6d9c33df4c09 790 //flushSerialBuffer();
umeume 8:6d9c33df4c09 791 //gps_setting();
umeume 8:6d9c33df4c09 792 //height_1 = 29.4;
umeume 8:6d9c33df4c09 793 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 794 char hei1[8]= {0x00},hei2[8]= {0x00};
umeume 8:6d9c33df4c09 795 int i=0,j=0;
umeume 8:6d9c33df4c09 796 while (serial.readable()) {
umeume 8:6d9c33df4c09 797 serial.getc();
umeume 8:6d9c33df4c09 798 }
umeume 8:6d9c33df4c09 799 loop:
umeume 8:6d9c33df4c09 800 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 801 for(j=0; j<5; j++) {
umeume 8:6d9c33df4c09 802 gps_data[1][j]=serial.getc();
umeume 8:6d9c33df4c09 803 }
umeume 8:6d9c33df4c09 804 if((gps_data[1][2]==0x47)&(gps_data[1][3]==0x47)&(gps_data[1][4]==0x41)) {
umeume 8:6d9c33df4c09 805 for(j=0; j<1; j++) {
umeume 8:6d9c33df4c09 806 if(j==0) {
umeume 8:6d9c33df4c09 807 i=0;
umeume 8:6d9c33df4c09 808 while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 809 //pc.putc(gps_data[j+1][i+5]);
umeume 8:6d9c33df4c09 810 i++;
umeume 8:6d9c33df4c09 811 }
umeume 8:6d9c33df4c09 812 gps_data[j+1][i+5]='\0';
umeume 8:6d9c33df4c09 813 i=0;
umeume 8:6d9c33df4c09 814 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 815 } else {
umeume 8:6d9c33df4c09 816 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 817 i=0;
umeume 8:6d9c33df4c09 818 while((gps_data[j+1][i] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 819 //pc.putc(gps_data[j+1][i]);
umeume 8:6d9c33df4c09 820 i++;
umeume 8:6d9c33df4c09 821 }
umeume 8:6d9c33df4c09 822 gps_data[j+1][i]='\0';
umeume 8:6d9c33df4c09 823 i=0;
umeume 8:6d9c33df4c09 824 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 825 }
umeume 8:6d9c33df4c09 826 }
umeume 8:6d9c33df4c09 827 } else {
umeume 8:6d9c33df4c09 828 goto loop;
umeume 8:6d9c33df4c09 829 }
umeume 8:6d9c33df4c09 830 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 831 //hokui
umeume 8:6d9c33df4c09 832 d_hokui=int(hokui/100);
umeume 8:6d9c33df4c09 833 m_hokui=(hokui-d_hokui*100);
umeume 8:6d9c33df4c09 834 //m_hokui=(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 835 g_hokui=d_hokui+(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 836 //printf("%f\r\n",hokui);
umeume 8:6d9c33df4c09 837 sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 838 sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 839 sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
umeume 8:6d9c33df4c09 840 sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 841
umeume 8:6d9c33df4c09 842 //tokei
umeume 8:6d9c33df4c09 843 d_tokei=int(tokei/100);
umeume 8:6d9c33df4c09 844 m_tokei=(tokei-d_tokei*100);
umeume 8:6d9c33df4c09 845 //m_tokei=(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 846 g_tokei=d_tokei+(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 847 //printf("%f\r\n",tokei);
umeume 8:6d9c33df4c09 848 sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 849 sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 850 sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
umeume 8:6d9c33df4c09 851 sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 852
umeume 8:6d9c33df4c09 853 m_height = int(_altitude);
umeume 8:6d9c33df4c09 854 cm_height = int((height_1-m_height)*100 );
umeume 8:6d9c33df4c09 855 // printf("%f\r\n",_altitude);
umeume 8:6d9c33df4c09 856 sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
umeume 8:6d9c33df4c09 857 sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
umeume 8:6d9c33df4c09 858
umeume 8:6d9c33df4c09 859 lat[0] = gph1[0];
umeume 8:6d9c33df4c09 860 lat[1] = gph1[1];
umeume 8:6d9c33df4c09 861 lat[2] = gph2[0];
umeume 8:6d9c33df4c09 862 lat[3] = gph2[1];
umeume 8:6d9c33df4c09 863 lat[4] = gph3[0];
umeume 8:6d9c33df4c09 864 lat[5] = gph3[1];
umeume 8:6d9c33df4c09 865 lat[6] = gph4[0];
umeume 8:6d9c33df4c09 866 lat[7] = gph4[1];
umeume 8:6d9c33df4c09 867 log[0] = gpt1[0];
umeume 8:6d9c33df4c09 868 log[1] = gpt1[1];
umeume 8:6d9c33df4c09 869 log[2] = gpt2[0];
umeume 8:6d9c33df4c09 870 log[3] = gpt2[1];
umeume 8:6d9c33df4c09 871 log[4] = gpt3[0];
umeume 8:6d9c33df4c09 872 log[5] = gpt3[1];
umeume 8:6d9c33df4c09 873 log[6] = gpt4[0];
umeume 8:6d9c33df4c09 874 log[7] = gpt4[1];
umeume 8:6d9c33df4c09 875 height[0] = hei1[0];
umeume 8:6d9c33df4c09 876 height[1] = hei1[1];
umeume 8:6d9c33df4c09 877 height[2] = hei2[0];
umeume 8:6d9c33df4c09 878 height[3] = hei2[1];
umeume 8:6d9c33df4c09 879 }
umeume 8:6d9c33df4c09 880 *dsize1 = 8;
umeume 8:6d9c33df4c09 881 *dsize2 = 4;
umeume 8:6d9c33df4c09 882 }
umeume 8:6d9c33df4c09 883 /*
umeume 8:6d9c33df4c09 884 void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
umeume 8:6d9c33df4c09 885 {
umeume 8:6d9c33df4c09 886 flushSerialBuffer();
umeume 8:6d9c33df4c09 887 //gps_setting();
umeume 8:6d9c33df4c09 888 //height_1 = 29.4;
umeume 8:6d9c33df4c09 889 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 890 char hei1[8]= {0x00},hei2[8]= {0x00};
umeume 8:6d9c33df4c09 891 int i=0,j=0;
umeume 8:6d9c33df4c09 892 while (serial.readable()) {
umeume 8:6d9c33df4c09 893 serial.getc();
umeume 8:6d9c33df4c09 894 }
umeume 8:6d9c33df4c09 895 loop:
umeume 8:6d9c33df4c09 896 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 897 for(j=0; j<5; j++) {
umeume 8:6d9c33df4c09 898 gps_data[1][j]=serial.getc();
umeume 8:6d9c33df4c09 899 }
umeume 8:6d9c33df4c09 900 if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)) {
umeume 8:6d9c33df4c09 901 for(j=0; j<1; j++) {
umeume 8:6d9c33df4c09 902 if(j==0) {
umeume 8:6d9c33df4c09 903 i=0;
umeume 8:6d9c33df4c09 904 while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 905 //pc.putc(gps_data[j+1][i+5]);
umeume 8:6d9c33df4c09 906 i++;
umeume 8:6d9c33df4c09 907 }
umeume 8:6d9c33df4c09 908 gps_data[j+1][i+5]='\0';
umeume 8:6d9c33df4c09 909 i=0;
umeume 8:6d9c33df4c09 910 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 911 } else {
umeume 8:6d9c33df4c09 912 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 913 i=0;
umeume 8:6d9c33df4c09 914 while((gps_data[j+1][i] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 915 //pc.putc(gps_data[j+1][i]);
umeume 8:6d9c33df4c09 916 i++;
umeume 8:6d9c33df4c09 917 }
umeume 8:6d9c33df4c09 918 gps_data[j+1][i]='\0';
umeume 8:6d9c33df4c09 919 i=0;
umeume 8:6d9c33df4c09 920 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 921 }
umeume 8:6d9c33df4c09 922 }
umeume 8:6d9c33df4c09 923 } else {
umeume 8:6d9c33df4c09 924 goto loop;
umeume 8:6d9c33df4c09 925 }
umeume 8:6d9c33df4c09 926 if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1) {
umeume 8:6d9c33df4c09 927 //hokui
umeume 8:6d9c33df4c09 928 d_hokui=int(hokui/100);
umeume 8:6d9c33df4c09 929 m_hokui=(hokui-d_hokui*100);
umeume 8:6d9c33df4c09 930 //m_hokui=(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 931 g_hokui=d_hokui+(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 932 sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 933 sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 934 sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
umeume 8:6d9c33df4c09 935 sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 936
umeume 8:6d9c33df4c09 937 //tokei
umeume 8:6d9c33df4c09 938 d_tokei=int(tokei/100);
umeume 8:6d9c33df4c09 939 m_tokei=(tokei-d_tokei*100);
umeume 8:6d9c33df4c09 940 //m_tokei=(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 941 g_tokei=d_tokei+(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 942 sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 943 sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 944 sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
umeume 8:6d9c33df4c09 945 sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 946
umeume 8:6d9c33df4c09 947 m_height = int(height_1);
umeume 8:6d9c33df4c09 948 cm_height = int((height_1-m_height)*100 );
umeume 8:6d9c33df4c09 949 //printf("%d\r\n",cm_height);
umeume 8:6d9c33df4c09 950 sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
umeume 8:6d9c33df4c09 951 sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
umeume 8:6d9c33df4c09 952
umeume 8:6d9c33df4c09 953 lat[0] = gph1[0];
umeume 8:6d9c33df4c09 954 lat[1] = gph1[1];
umeume 8:6d9c33df4c09 955 lat[2] = gph2[0];
umeume 8:6d9c33df4c09 956 lat[3] = gph2[1];
umeume 8:6d9c33df4c09 957 lat[4] = gph3[0];
umeume 8:6d9c33df4c09 958 lat[5] = gph3[1];
umeume 8:6d9c33df4c09 959 lat[6] = gph4[0];
umeume 8:6d9c33df4c09 960 lat[7] = gph4[1];
umeume 8:6d9c33df4c09 961 log[0] = gpt1[0];
umeume 8:6d9c33df4c09 962 log[1] = gpt1[1];
umeume 8:6d9c33df4c09 963 log[2] = gpt2[0];
umeume 8:6d9c33df4c09 964 log[3] = gpt2[1];
umeume 8:6d9c33df4c09 965 log[4] = gpt3[0];
umeume 8:6d9c33df4c09 966 log[5] = gpt3[1];
umeume 8:6d9c33df4c09 967 log[6] = gpt4[0];
umeume 8:6d9c33df4c09 968 log[7] = gpt4[1];
umeume 8:6d9c33df4c09 969 height[0] = hei1[0];
umeume 8:6d9c33df4c09 970 height[1] = hei1[1];
umeume 8:6d9c33df4c09 971 height[2] = hei2[0];
umeume 8:6d9c33df4c09 972 height[3] = hei2[1];
umeume 8:6d9c33df4c09 973 }
umeume 8:6d9c33df4c09 974 *dsize1 = 8;
umeume 8:6d9c33df4c09 975 *dsize2 = 4;
umeume 8:6d9c33df4c09 976 }*/
umeume 8:6d9c33df4c09 977 /*
umeume 8:6d9c33df4c09 978 void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *serial_check ,int *dsize1, int *dsize2)
umeume 8:6d9c33df4c09 979 {
umeume 8:6d9c33df4c09 980 int quality=0,stnum=0;
umeume 8:6d9c33df4c09 981 char ns='A',ew='B',aunit='m';
umeume 8:6d9c33df4c09 982 float time=0.0,latitude=0.0,longitude=0.0,hacu=0.0,altitude=0.0;
umeume 8:6d9c33df4c09 983
umeume 8:6d9c33df4c09 984 flushSerialBuffer();
umeume 8:6d9c33df4c09 985 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 986 char hei1[8]= {0x00},hei2[8]= {0x00};
umeume 8:6d9c33df4c09 987 int ite = 0;
umeume 8:6d9c33df4c09 988 while(serial.getc()!='$') {
umeume 8:6d9c33df4c09 989 ite++;
umeume 8:6d9c33df4c09 990 if(ite==10000) break;
umeume 8:6d9c33df4c09 991 }
umeume 8:6d9c33df4c09 992 for(int i=0; i<5; i++) {
umeume 8:6d9c33df4c09 993 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 994 }
umeume 8:6d9c33df4c09 995 if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
umeume 8:6d9c33df4c09 996 for(int j=0; j<6; j++) {
umeume 8:6d9c33df4c09 997 if(j==0) {
umeume 8:6d9c33df4c09 998 for(int i=5; i<256; i++) {
umeume 8:6d9c33df4c09 999 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 1000 if(msg[i] == '\r') {
umeume 8:6d9c33df4c09 1001 msg[i] = 0;
umeume 8:6d9c33df4c09 1002 break;
umeume 8:6d9c33df4c09 1003 }
umeume 8:6d9c33df4c09 1004 }
umeume 8:6d9c33df4c09 1005 } else {
umeume 8:6d9c33df4c09 1006 for(int i=0; i<256; i++) {
umeume 8:6d9c33df4c09 1007 msgd[i] = serial.getc();
umeume 8:6d9c33df4c09 1008 if(msgd[i] == '\r') {
umeume 8:6d9c33df4c09 1009 break;
umeume 8:6d9c33df4c09 1010 }
umeume 8:6d9c33df4c09 1011 }
umeume 8:6d9c33df4c09 1012 if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
umeume 8:6d9c33df4c09 1013 break;
umeume 8:6d9c33df4c09 1014 }
umeume 8:6d9c33df4c09 1015 }
umeume 8:6d9c33df4c09 1016 }
umeume 8:6d9c33df4c09 1017 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 1018 if(!(quality)) {
umeume 8:6d9c33df4c09 1019
umeume 8:6d9c33df4c09 1020 *serial_check = 0;
umeume 8:6d9c33df4c09 1021 } else {
umeume 8:6d9c33df4c09 1022
umeume 8:6d9c33df4c09 1023 *serial_check = 1;
umeume 8:6d9c33df4c09 1024 }
umeume 8:6d9c33df4c09 1025 } else {
umeume 8:6d9c33df4c09 1026 printf("No Data");
umeume 8:6d9c33df4c09 1027 *serial_check = 2;
umeume 8:6d9c33df4c09 1028 }
umeume 8:6d9c33df4c09 1029 } else {
umeume 8:6d9c33df4c09 1030 *serial_check = 3;
umeume 8:6d9c33df4c09 1031 }
umeume 8:6d9c33df4c09 1032 //hokui
umeume 8:6d9c33df4c09 1033 d_hokui=int(latitude/100);
umeume 8:6d9c33df4c09 1034 m_hokui=(latitude-d_hokui*100);
umeume 8:6d9c33df4c09 1035 //m_hokui=(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 1036 g_hokui=d_hokui+(latitude-d_hokui*100)/60;
umeume 8:6d9c33df4c09 1037 printf("%f\r\n",latitude);
umeume 8:6d9c33df4c09 1038 sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 1039 sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 1040 sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
umeume 8:6d9c33df4c09 1041 sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 1042
umeume 8:6d9c33df4c09 1043 //tokei
umeume 8:6d9c33df4c09 1044 d_tokei=int(longitude/100);
umeume 8:6d9c33df4c09 1045 m_tokei=(longitude-d_tokei*100);
umeume 8:6d9c33df4c09 1046 //m_tokei=(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 1047 g_tokei=d_tokei+(longitude-d_tokei*100)/60;
umeume 8:6d9c33df4c09 1048 printf("%f\r\n",longitude);
umeume 8:6d9c33df4c09 1049 sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 1050 sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 1051 sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
umeume 8:6d9c33df4c09 1052 sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 1053
umeume 8:6d9c33df4c09 1054 m_height = int(altitude);
umeume 8:6d9c33df4c09 1055 cm_height = int((altitude-m_height)*100 );
umeume 8:6d9c33df4c09 1056 printf("%f\r\n",altitude);
umeume 8:6d9c33df4c09 1057 sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
umeume 8:6d9c33df4c09 1058 sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
umeume 8:6d9c33df4c09 1059
umeume 8:6d9c33df4c09 1060 lat[0] = gph1[0];
umeume 8:6d9c33df4c09 1061 lat[1] = gph1[1];
umeume 8:6d9c33df4c09 1062 lat[2] = gph2[0];
umeume 8:6d9c33df4c09 1063 lat[3] = gph2[1];
umeume 8:6d9c33df4c09 1064 lat[4] = gph3[0];
umeume 8:6d9c33df4c09 1065 lat[5] = gph3[1];
umeume 8:6d9c33df4c09 1066 lat[6] = gph4[0];
umeume 8:6d9c33df4c09 1067 lat[7] = gph4[1];
umeume 8:6d9c33df4c09 1068 log[0] = gpt1[0];
umeume 8:6d9c33df4c09 1069 log[1] = gpt1[1];
umeume 8:6d9c33df4c09 1070 log[2] = gpt2[0];
umeume 8:6d9c33df4c09 1071 log[3] = gpt2[1];
umeume 8:6d9c33df4c09 1072 log[4] = gpt3[0];
umeume 8:6d9c33df4c09 1073 log[5] = gpt3[1];
umeume 8:6d9c33df4c09 1074 log[6] = gpt4[0];
umeume 8:6d9c33df4c09 1075 log[7] = gpt4[1];
umeume 8:6d9c33df4c09 1076 height[0] = hei1[0];
umeume 8:6d9c33df4c09 1077 height[1] = hei1[1];
umeume 8:6d9c33df4c09 1078 height[2] = hei2[0];
umeume 8:6d9c33df4c09 1079 height[3] = hei2[1];
umeume 8:6d9c33df4c09 1080 *dsize1 = 8;
umeume 8:6d9c33df4c09 1081 *dsize2 = 4;
umeume 8:6d9c33df4c09 1082 }*/