HEPTA-Sat Training 2019 / HeptaCamera_GPS

Cmera and GPS labrary

Dependents:   HEPTA_SENSOR HEPTA_SENSOR

HeptaCamera_GPS.cpp@27:98bb06a7ecf4, 2019-11-05 (annotated)

Committer:
HeptaSatTraining2019
Date:
Tue Nov 05 07:16:56 2019 +0000
Revision:
27:98bb06a7ecf4
Parent:
26:f2017248e4f2 
Compatible with south latitude and west longitude

Who changed what in which revision?

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