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@12:b298905009c9, 2017-09-06 (annotated)

Committer:
HEPTA
Date:
Wed Sep 06 06:07:39 2017 +0000
Revision:
12:b298905009c9
Parent:
10:d4ecef9ef8fd
Child:
13:e3ad89e65f66
Hepta Camera&GPS Library

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");
HEPTA 12:b298905009c9 612 //err = sync();
HEPTA 12:b298905009c9 613 err = (ErrorNumber)UnexpectedReply;
umeume 8:6d9c33df4c09 614 int count=0;
umeume 8:6d9c33df4c09 615 while(err) {
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 }
umeume 8:6d9c33df4c09 636 }
umeume 8:6d9c33df4c09 637
umeume 8:6d9c33df4c09 638 void HeptaCamera_GPS::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
umeume 8:6d9c33df4c09 639 {
umeume 8:6d9c33df4c09 640 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
umeume 8:6d9c33df4c09 641 for (int i = 0; i < CAPTURE_FRAMES; i++) {
umeume 8:6d9c33df4c09 642 char fname[64];
HEPTA 9:591580af98be 643 snprintf(fname, sizeof(fname), "/sd/test.jpg");
umeume 8:6d9c33df4c09 644 fp_jpeg = fopen(fname, "w");
umeume 8:6d9c33df4c09 645
umeume 8:6d9c33df4c09 646 err = getJpegSnapshotPicture();
umeume 8:6d9c33df4c09 647
umeume 8:6d9c33df4c09 648 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 649 printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
umeume 8:6d9c33df4c09 650 } else {
umeume 8:6d9c33df4c09 651 printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 652 }
umeume 8:6d9c33df4c09 653 fclose(fp_jpeg);
umeume 8:6d9c33df4c09 654 }
umeume 8:6d9c33df4c09 655 }
umeume 8:6d9c33df4c09 656
umeume 8:6d9c33df4c09 657 void HeptaCamera_GPS::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
umeume 8:6d9c33df4c09 658 {
umeume 8:6d9c33df4c09 659 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
umeume 8:6d9c33df4c09 660 for (int i = 0; i < CAPTURE_FRAMES; i++) {
umeume 8:6d9c33df4c09 661 char fname[64];
HEPTA 9:591580af98be 662 snprintf(fname, sizeof(fname), "/sd/test.txt");
umeume 8:6d9c33df4c09 663 fp_jpeg = fopen(fname, "w");
umeume 8:6d9c33df4c09 664
umeume 8:6d9c33df4c09 665 err = getJpegSnapshotPicture_data();
umeume 8:6d9c33df4c09 666
umeume 8:6d9c33df4c09 667 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 668 printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
umeume 8:6d9c33df4c09 669 } else {
umeume 8:6d9c33df4c09 670 printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 671 }
umeume 8:6d9c33df4c09 672 fclose(fp_jpeg);
umeume 8:6d9c33df4c09 673 }
umeume 8:6d9c33df4c09 674 }
umeume 8:6d9c33df4c09 675
umeume 8:6d9c33df4c09 676 void HeptaCamera_GPS::initialize(Baud baud,JpegResolution jr)
umeume 8:6d9c33df4c09 677 {
umeume 8:6d9c33df4c09 678 HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;//ErrorNumber define
umeume 8:6d9c33df4c09 679 err = init(baud, jr);//
umeume 8:6d9c33df4c09 680 if (HeptaCamera_GPS::NoError == err) {
umeume 8:6d9c33df4c09 681 printf("[ OK ] : Camera::init\r\n");
umeume 8:6d9c33df4c09 682 setmbedBaud(baud);
umeume 8:6d9c33df4c09 683 } else {
umeume 8:6d9c33df4c09 684 printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
umeume 8:6d9c33df4c09 685 }
umeume 8:6d9c33df4c09 686
umeume 8:6d9c33df4c09 687 }
umeume 8:6d9c33df4c09 688
umeume 8:6d9c33df4c09 689 //*********************serial*********************//
umeume 8:6d9c33df4c09 690 void HeptaCamera_GPS::gps_setting(void)
umeume 8:6d9c33df4c09 691 {
umeume 8:6d9c33df4c09 692 CAM_SW = 0;
umeume 8:6d9c33df4c09 693 GPS_SW = 1;
HEPTA 10:d4ecef9ef8fd 694 flushSerialBuffer();
umeume 8:6d9c33df4c09 695 serial._baud(9600);
umeume 8:6d9c33df4c09 696 serial.setTimeout(9999);
umeume 8:6d9c33df4c09 697 }
umeume 8:6d9c33df4c09 698
umeume 8:6d9c33df4c09 699
umeume 8:6d9c33df4c09 700 char HeptaCamera_GPS::getc()
umeume 8:6d9c33df4c09 701 {
umeume 8:6d9c33df4c09 702 c = serial.getc();
umeume 8:6d9c33df4c09 703 return c;
umeume 8:6d9c33df4c09 704 }
umeume 8:6d9c33df4c09 705 int HeptaCamera_GPS::readable()
umeume 8:6d9c33df4c09 706 {
umeume 8:6d9c33df4c09 707 i = serial.readable();
umeume 8:6d9c33df4c09 708 return i;
umeume 8:6d9c33df4c09 709 }
umeume 8:6d9c33df4c09 710 void HeptaCamera_GPS::flushSerialBuffer(void)
umeume 8:6d9c33df4c09 711 {
umeume 8:6d9c33df4c09 712 ite = 0;
umeume 8:6d9c33df4c09 713 while (serial.readable()) {
umeume 8:6d9c33df4c09 714 serial.getc();
umeume 8:6d9c33df4c09 715 ite++;
umeume 8:6d9c33df4c09 716 if(ite==100) {
umeume 8:6d9c33df4c09 717 break;
umeume 8:6d9c33df4c09 718 };
umeume 8:6d9c33df4c09 719 }
umeume 8:6d9c33df4c09 720 return;
umeume 8:6d9c33df4c09 721 }
umeume 8:6d9c33df4c09 722 void HeptaCamera_GPS::gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *serial_check)
umeume 8:6d9c33df4c09 723 {
umeume 8:6d9c33df4c09 724 int ite = 0;
umeume 8:6d9c33df4c09 725 while(serial.getc()!='$') {
umeume 8:6d9c33df4c09 726 ite++;
umeume 8:6d9c33df4c09 727 if(ite==10000) break;
umeume 8:6d9c33df4c09 728 }
umeume 8:6d9c33df4c09 729 for(int i=0; i<5; i++) {
umeume 8:6d9c33df4c09 730 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 731 }
umeume 8:6d9c33df4c09 732 if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
umeume 8:6d9c33df4c09 733 for(int j=0; j<6; j++) {
umeume 8:6d9c33df4c09 734 if(j==0) {
umeume 8:6d9c33df4c09 735 for(int i=5; i<256; i++) {
umeume 8:6d9c33df4c09 736 msg[i] = serial.getc();
umeume 8:6d9c33df4c09 737 if(msg[i] == '\r') {
umeume 8:6d9c33df4c09 738 msg[i] = 0;
umeume 8:6d9c33df4c09 739 break;
umeume 8:6d9c33df4c09 740 }
umeume 8:6d9c33df4c09 741 }
umeume 8:6d9c33df4c09 742 } else {
umeume 8:6d9c33df4c09 743 for(int i=0; i<256; i++) {
umeume 8:6d9c33df4c09 744 msgd[i] = serial.getc();
umeume 8:6d9c33df4c09 745 if(msgd[i] == '\r') {
umeume 8:6d9c33df4c09 746 break;
umeume 8:6d9c33df4c09 747 }
umeume 8:6d9c33df4c09 748 }
umeume 8:6d9c33df4c09 749 if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
umeume 8:6d9c33df4c09 750 break;
umeume 8:6d9c33df4c09 751 }
umeume 8:6d9c33df4c09 752 }
umeume 8:6d9c33df4c09 753 }
umeume 8:6d9c33df4c09 754 if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", time, latitude, ns, longitude, ew, quality, stnum, hacu, altitude, aunit) >= 1) {
umeume 8:6d9c33df4c09 755 if(!(quality)) {
umeume 8:6d9c33df4c09 756 //latitude(unit transformation)
umeume 8:6d9c33df4c09 757 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
umeume 8:6d9c33df4c09 758 //longitude(unit transformation)
umeume 8:6d9c33df4c09 759 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
umeume 8:6d9c33df4c09 760 *serial_check = 0;
umeume 8:6d9c33df4c09 761 } else {
umeume 8:6d9c33df4c09 762 //latitude(unit transformation)
umeume 8:6d9c33df4c09 763 *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
umeume 8:6d9c33df4c09 764 //longitude(unit transformation)
umeume 8:6d9c33df4c09 765 *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
umeume 8:6d9c33df4c09 766 *serial_check = 1;
umeume 8:6d9c33df4c09 767 }
umeume 8:6d9c33df4c09 768 } else {
umeume 8:6d9c33df4c09 769 printf("No Data");
umeume 8:6d9c33df4c09 770 *serial_check = 2;
umeume 8:6d9c33df4c09 771 }
umeume 8:6d9c33df4c09 772 } else {
umeume 8:6d9c33df4c09 773 *serial_check = 3;
umeume 8:6d9c33df4c09 774 }
umeume 8:6d9c33df4c09 775 }
umeume 8:6d9c33df4c09 776
umeume 8:6d9c33df4c09 777 void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
umeume 8:6d9c33df4c09 778 {
umeume 8:6d9c33df4c09 779 char gph1[8]= {0x00},gph2[8]= {0x00},gph3[8]= {0x00},gph4[8]= {0x00},gpt1[8]= {0x00},gpt2[8]= {0x00},gpt3[8]= {0x00},gpt4[8]= {0x00};
umeume 8:6d9c33df4c09 780 char hei1[8]= {0x00},hei2[8]= {0x00};
umeume 8:6d9c33df4c09 781 int i=0,j=0;
umeume 8:6d9c33df4c09 782 while (serial.readable()) {
umeume 8:6d9c33df4c09 783 serial.getc();
umeume 8:6d9c33df4c09 784 }
umeume 8:6d9c33df4c09 785 loop:
umeume 8:6d9c33df4c09 786 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 787 for(j=0; j<5; j++) {
umeume 8:6d9c33df4c09 788 gps_data[1][j]=serial.getc();
umeume 8:6d9c33df4c09 789 }
umeume 8:6d9c33df4c09 790 if((gps_data[1][2]==0x47)&(gps_data[1][3]==0x47)&(gps_data[1][4]==0x41)) {
umeume 8:6d9c33df4c09 791 for(j=0; j<1; j++) {
umeume 8:6d9c33df4c09 792 if(j==0) {
umeume 8:6d9c33df4c09 793 i=0;
umeume 8:6d9c33df4c09 794 while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 795 //pc.putc(gps_data[j+1][i+5]);
umeume 8:6d9c33df4c09 796 i++;
umeume 8:6d9c33df4c09 797 }
umeume 8:6d9c33df4c09 798 gps_data[j+1][i+5]='\0';
umeume 8:6d9c33df4c09 799 i=0;
umeume 8:6d9c33df4c09 800 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 801 } else {
umeume 8:6d9c33df4c09 802 while(serial.getc()!='$') {}
umeume 8:6d9c33df4c09 803 i=0;
umeume 8:6d9c33df4c09 804 while((gps_data[j+1][i] = serial.getc()) != '\r') {
umeume 8:6d9c33df4c09 805 //pc.putc(gps_data[j+1][i]);
umeume 8:6d9c33df4c09 806 i++;
umeume 8:6d9c33df4c09 807 }
umeume 8:6d9c33df4c09 808 gps_data[j+1][i]='\0';
umeume 8:6d9c33df4c09 809 i=0;
umeume 8:6d9c33df4c09 810 //pc.printf("\n\r");
umeume 8:6d9c33df4c09 811 }
umeume 8:6d9c33df4c09 812 }
umeume 8:6d9c33df4c09 813 } else {
umeume 8:6d9c33df4c09 814 goto loop;
umeume 8:6d9c33df4c09 815 }
umeume 8:6d9c33df4c09 816 if( sscanf(gps_data[1],"GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &time, &hokui, &ns, &tokei, &ew, &_quality, &_stnum, &_hacu, &_altitude, &_aunit) >= 1) {
umeume 8:6d9c33df4c09 817 //hokui
umeume 8:6d9c33df4c09 818 d_hokui=int(hokui/100);
umeume 8:6d9c33df4c09 819 m_hokui=(hokui-d_hokui*100);
umeume 8:6d9c33df4c09 820 //m_hokui=(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 821 g_hokui=d_hokui+(hokui-d_hokui*100)/60;
umeume 8:6d9c33df4c09 822 //printf("%f\r\n",hokui);
umeume 8:6d9c33df4c09 823 sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 824 sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
umeume 8:6d9c33df4c09 825 sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
umeume 8:6d9c33df4c09 826 sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 827
umeume 8:6d9c33df4c09 828 //tokei
umeume 8:6d9c33df4c09 829 d_tokei=int(tokei/100);
umeume 8:6d9c33df4c09 830 m_tokei=(tokei-d_tokei*100);
umeume 8:6d9c33df4c09 831 //m_tokei=(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 832 g_tokei=d_tokei+(tokei-d_tokei*100)/60;
umeume 8:6d9c33df4c09 833 //printf("%f\r\n",tokei);
umeume 8:6d9c33df4c09 834 sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 835 sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
umeume 8:6d9c33df4c09 836 sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
umeume 8:6d9c33df4c09 837 sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
umeume 8:6d9c33df4c09 838
umeume 8:6d9c33df4c09 839 m_height = int(_altitude);
HEPTA 9:591580af98be 840 cm_height = int((_altitude-m_height)*100 );
umeume 8:6d9c33df4c09 841 // printf("%f\r\n",_altitude);
umeume 8:6d9c33df4c09 842 sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
umeume 8:6d9c33df4c09 843 sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
umeume 8:6d9c33df4c09 844
umeume 8:6d9c33df4c09 845 lat[0] = gph1[0];
umeume 8:6d9c33df4c09 846 lat[1] = gph1[1];
umeume 8:6d9c33df4c09 847 lat[2] = gph2[0];
umeume 8:6d9c33df4c09 848 lat[3] = gph2[1];
umeume 8:6d9c33df4c09 849 lat[4] = gph3[0];
umeume 8:6d9c33df4c09 850 lat[5] = gph3[1];
umeume 8:6d9c33df4c09 851 lat[6] = gph4[0];
umeume 8:6d9c33df4c09 852 lat[7] = gph4[1];
umeume 8:6d9c33df4c09 853 log[0] = gpt1[0];
umeume 8:6d9c33df4c09 854 log[1] = gpt1[1];
umeume 8:6d9c33df4c09 855 log[2] = gpt2[0];
umeume 8:6d9c33df4c09 856 log[3] = gpt2[1];
umeume 8:6d9c33df4c09 857 log[4] = gpt3[0];
umeume 8:6d9c33df4c09 858 log[5] = gpt3[1];
umeume 8:6d9c33df4c09 859 log[6] = gpt4[0];
umeume 8:6d9c33df4c09 860 log[7] = gpt4[1];
umeume 8:6d9c33df4c09 861 height[0] = hei1[0];
umeume 8:6d9c33df4c09 862 height[1] = hei1[1];
umeume 8:6d9c33df4c09 863 height[2] = hei2[0];
umeume 8:6d9c33df4c09 864 height[3] = hei2[1];
umeume 8:6d9c33df4c09 865 }
umeume 8:6d9c33df4c09 866 *dsize1 = 8;
umeume 8:6d9c33df4c09 867 *dsize2 = 4;
umeume 8:6d9c33df4c09 868 }