CLTP 8
test
Dependents: HEPTA_assembly template HEPTA_GS
Revision 8:6d9c33df4c09, committed 2017-08-23
- Comitter:
- umeume
- Date:
- Wed Aug 23 06:18:42 2017 +0000
- Parent:
- 7:a41100627f55
- Commit message:
- test
Changed in this revision
diff -r a41100627f55 -r 6d9c33df4c09 HeptaCamera_GPS.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaCamera_GPS.cpp Wed Aug 23 06:18:42 2017 +0000
@@ -0,0 +1,1082 @@
+#include "HeptaCamera_GPS.h"
+static FILE *fp_jpeg;
+#define WAITIDLE waitIdle
+#define SENDFUNC sendBytes
+#define RECVFUNC recvBytes
+#define WAITFUNC waitRecv
+int num=0;
+/**
+ * Constructor.
+ *
+ * @param tx A pin for transmit.
+ * @param rx A pin for receive.
+ * @param baud Baud rate. (Default is Baud14400.)
+ */
+HeptaCamera_GPS::HeptaCamera_GPS(PinName tx, PinName rx, PinName CAM, PinName GPS) : serial(tx, rx), CAM_SW(CAM), GPS_SW(GPS)
+{
+ //serial.baud(baud);
+}
+
+/**
+ * Destructor.
+ */
+HeptaCamera_GPS::~HeptaCamera_GPS()
+{
+}
+
+/**
+ * Make a sync. for baud rate.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sync()
+{
+ for (int i = 0; i < SYNCMAX; i++) {
+ if (NoError == sendSync()) {
+ if (NoError == recvAckOrNck()) {
+ if (NoError == recvSync()) {
+ if (NoError == sendAck(0x0D, 0x00)) {
+ /*
+ * After synchronization, the camera needs a little time for AEC and AGC to be stable.
+ * Users should wait for 1-2 seconds before capturing the first picture.
+ */
+ wait(2.0);
+ return NoError;
+ }
+ }
+ }
+ }
+ wait_ms(50);
+ }
+ return UnexpectedReply;
+}
+
+/**
+ * Initialize.
+ *
+ * @param baud Camera Interface Speed.
+ * @param jr JPEG resolution.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::init(Baud baud,JpegResolution jr)
+{
+ int i ;
+ ErrorNumber en;
+ WAITIDLE();
+ setmbedBaud((Baud)(0x07)) ;
+
+ for ( i = 1 ; i < 7 ; i++ ) {
+ if ( NoError == sendSync() ) {
+ if ( NoError == recvAckOrNck() ) {
+ if ( NoError == recvSync() ) {
+ if ( NoError == sendAck(0x0D, 0x00) ) {
+ en = sendInitial(baud,jr);
+ if (NoError != en) {
+ return en;
+ }
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+ wait_ms(50) ;
+ setmbedBaud(baud);
+ //wait_ms(50) ;
+ static bool alreadySetupPackageSize = false;
+ if (!alreadySetupPackageSize) {
+ en = sendSetPackageSize(packageSize);
+ if (NoError != en) {
+ return en;
+ }
+ WAITFUNC();
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+ alreadySetupPackageSize = true;
+ }
+
+ wait(2.0);
+ return (ErrorNumber)NoError;
+ /*
+ * After synchronization, the camera needs a little time for AEC and AGC to be stable.
+ * Users should wait for 1-2 seconds before capturing the first picture.
+ */
+ }
+ }
+ } else {
+ setmbedBaud((Baud)(i+1)) ;
+ }
+ }
+ wait_ms(50);
+ }
+ return UnexpectedReply;
+}
+
+
+/**
+ * Get JPEG snapshot picture.
+ *
+ * @param func A pointer to a callback function.
+ * You can block this function until saving the image datas.
+ * @return Status of the error.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture()
+{
+ WAITIDLE();
+ ErrorNumber en;
+
+
+ en = sendSnapshot();
+ if (NoError != en) {
+ return en;
+ }
+ WAITFUNC();
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+
+ en = sendGetPicture();
+ if (NoError != en) {
+ return en;
+ }
+ WAITFUNC();
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+
+ /*
+ * Data : snapshot picture
+ */
+ uint32_t length = 0;
+ WAITFUNC();
+ en = recvData(&length);
+ if (NoError != en) {
+ return en;
+ }
+ en = sendAck(0x00, 0);
+ if (NoError != en) {
+ return en;
+ }
+
+ char databuf[packageSize - 6];
+ uint16_t pkg_total = length / (packageSize - 6);
+ for (int i = 0; i <= (int)pkg_total; i++) {
+ uint16_t checksum = 0;
+ // ID.
+ char idbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(idbuf, sizeof(idbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ checksum += idbuf[0];
+ checksum += idbuf[1];
+ uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
+ if (id != i) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ // Size of the data.
+ char dsbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ // Received the data.
+ checksum += dsbuf[0];
+ checksum += dsbuf[1];
+ uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
+ WAITFUNC();
+ if (!RECVFUNC(&databuf[0], ds)) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ for (int j = 0; j < ds; j++) {
+ checksum += databuf[j];
+ }
+
+ // Verify code.
+ char vcbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
+ if (vc != (checksum & 0xff)) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ /*
+ * Call a call back function.
+ * You can block this function while working.
+ */
+ size_t siz = ds;
+ for (int ii = 0; ii < (int)siz; ii++) {
+ fprintf(fp_jpeg, "%c", databuf[ii]);
+ }
+ /*
+ * We should wait for camera working before reply a ACK.
+ */
+ wait_ms(100);
+ en = sendAck(0x00, 1 + i);
+ if (NoError != en) {
+ return en;
+ }
+ }
+
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::getJpegSnapshotPicture_data()
+{
+ WAITIDLE();
+ ErrorNumber en;
+
+
+ en = sendSnapshot();
+ if (NoError != en) {
+ return en;
+ }
+ WAITFUNC();
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+
+ en = sendGetPicture();
+ if (NoError != en) {
+ return en;
+ }
+ WAITFUNC();
+ en = recvAckOrNck();
+ if (NoError != en) {
+ return en;
+ }
+
+ /*
+ * Data : snapshot picture
+ */
+ uint32_t length = 0;
+ WAITFUNC();
+ en = recvData(&length);
+ if (NoError != en) {
+ return en;
+ }
+ en = sendAck(0x00, 0);
+ if (NoError != en) {
+ return en;
+ }
+
+ char databuf[packageSize - 6];
+ uint16_t pkg_total = length / (packageSize - 6);
+ for (int i = 0; i <= (int)pkg_total; i++) {
+ uint16_t checksum = 0;
+ // ID.
+ char idbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(idbuf, sizeof(idbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ checksum += idbuf[0];
+ checksum += idbuf[1];
+ uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
+ if (id != i) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ // Size of the data.
+ char dsbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ // Received the data.
+ checksum += dsbuf[0];
+ checksum += dsbuf[1];
+ uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
+ WAITFUNC();
+ if (!RECVFUNC(&databuf[0], ds)) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ for (int j = 0; j < ds; j++) {
+ checksum += databuf[j];
+ }
+
+ // Verify code.
+ char vcbuf[2];
+ WAITFUNC();
+ if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
+ if (vc != (checksum & 0xff)) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+
+ /*
+ * Call a call back function.
+ * You can block this function while working.
+ */
+ size_t siz = ds;
+ for (int ii = 0; ii < (int)siz; ii++) {
+ fprintf(fp_jpeg, "%02X ", databuf[ii]);
+ if(++num%16==0) fprintf(fp_jpeg,"\r\n");
+ }
+ /*
+ * We should wait for camera working before reply a ACK.
+ */
+ wait_ms(100);
+ en = sendAck(0x00, 1 + i);
+ if (NoError != en) {
+ return en;
+ }
+ }
+
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendInitial(Baud baud, JpegResolution jr)
+{
+ char send[COMMAND_LENGTH];
+
+ send[0] = 0xAA;
+ send[1] = 0x01;
+ send[2] = (char)baud;
+ send[3] = 0x07;
+ send[4] = 0x00;
+ send[5] = (char)jr;
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendGetPicture(void)
+{
+ char send[COMMAND_LENGTH];
+
+ send[0] = 0xAA;
+ send[1] = 0x04;
+ send[2] = 0x01;
+ send[3] = 0x00;
+ send[4] = 0x00;
+ send[5] = 0x00;
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSnapshot(void)
+{
+ char send[COMMAND_LENGTH];
+ send[0] = 0xAA;
+ send[1] = 0x05;
+ send[2] = 0x00;
+ send[3] = 0x00;
+ send[4] = 0x00;
+ send[5] = 0x00;
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSetPackageSize(uint16_t packageSize)
+{
+ char send[COMMAND_LENGTH];
+ send[0] = 0xAA;
+ send[1] = 0x06;
+ send[2] = 0x08;
+ send[3] = (packageSize >> 0) & 0xff;
+ send[4] = (packageSize >> 8) & 0xff;
+ send[5] = 0x00;
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+ return (ErrorNumber)NoError;
+}
+
+void HeptaCamera_GPS::setmbedBaud(Baud baud)
+{
+ switch((int)baud) {
+ case 0x02:
+ serial._baud(460800);
+ break;
+ case 0x03:
+ serial._baud(230400);
+ break;
+ case 0x04:
+ serial._baud(115200);
+ break;
+ case 0x05:
+ serial._baud(57600);
+ break;
+ case 0x06:
+ serial._baud((int)28800);
+ break;
+ case 0x07:
+ serial._baud(14400);
+ break;
+ default:
+ serial._baud(14400);
+ }
+}
+
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendReset(ResetType specialReset)
+{
+ char send[COMMAND_LENGTH];
+ send[0] = 0xAA;
+ send[1] = 0x08;
+ send[2] = 0x00;
+ send[3] = 0x00;
+ send[4] = 0x00;
+ send[5] = specialReset;
+ /*
+ * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
+ */
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+
+ return (ErrorNumber)NoError;
+}
+
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvData(uint32_t *length)
+{
+ char recv[COMMAND_LENGTH];
+ if (!RECVFUNC(recv, sizeof(recv))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ if ((0xAA != recv[0]) || (0x0A != recv[1])) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ recv[2] = (char)0x01;
+ *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendSync()
+{
+ char send[COMMAND_LENGTH];
+ send[0] = 0xAA;
+ send[1] = 0x0D;
+ send[2] = 0x00;
+ send[3] = 0x00;
+ send[4] = 0x00;
+ send[5] = 0x00;
+
+
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+ return (ErrorNumber)NoError;
+}
+
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvSync()
+{
+ char recv[COMMAND_LENGTH];
+
+ if (!RECVFUNC(recv, sizeof(recv))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ if ((0xAA != recv[0]) || (0x0D != recv[1])) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ return (ErrorNumber)NoError;
+}
+
+/**
+ * Send ACK.
+ *
+ * @param commandId The command with that ID is acknowledged by this command.
+ * @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.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::sendAck(uint8_t commandId, uint16_t packageId)
+{
+ char send[COMMAND_LENGTH];
+
+ send[0] = 0xAA;
+ send[1] = 0x0E;
+ send[2] = commandId;
+ send[3] = 0x00; // ACK counter is not used.
+ send[4] = (packageId >> 0) & 0xff;
+ send[5] = (packageId >> 8) & 0xff;
+ if (!SENDFUNC(send, sizeof(send))) {
+ return (ErrorNumber)SendRegisterTimeout;
+ }
+ return (ErrorNumber)NoError;
+}
+
+/**
+ * Receive ACK or NCK.
+ *
+ * @return Error number.
+ */
+HeptaCamera_GPS::ErrorNumber HeptaCamera_GPS::recvAckOrNck()
+{
+ char recv[COMMAND_LENGTH];
+
+ if (!RECVFUNC(recv, sizeof(recv))) {
+ return (ErrorNumber)UnexpectedReply;
+ }
+ if ((0xAA == recv[0]) && (0x0E == recv[1])) {
+ return (ErrorNumber)NoError;
+ }
+ if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) {
+ return (ErrorNumber)NoError;
+ }
+ if ((0xAA == recv[0]) && (0x0F == recv[1])) {
+ return (ErrorNumber)recv[4];
+ }
+ return (ErrorNumber)UnexpectedReply;
+}
+
+/**
+ * Send bytes to camera module.
+ *
+ * @param buf Pointer to the data buffer.
+ * @param len Length of the data buffer.
+ *
+ * @return True if the data sended.
+ */
+bool HeptaCamera_GPS::sendBytes(char *buf, size_t len, int timeout_us)
+{
+ for (uint32_t i = 0; i < (uint32_t)len; i++) {
+ int cnt = 0;
+ while (!serial.writeable()) {
+ wait_us(1);
+ cnt++;
+ if (timeout_us < cnt) {
+ return false;
+ }
+ }
+ serial.putc(buf[i]);
+ }
+ return true;
+}
+
+
+bool HeptaCamera_GPS::recvBytes(char *buf, size_t len, int timeout_us)
+{
+ for (uint32_t i = 0; i < (uint32_t)len; i++) {
+ int cnt = 0;
+ while (!serial.readable()) {
+ wait_us(1);
+ cnt++;
+ if (timeout_us < cnt) {
+ return false;
+ }
+ }
+ buf[i] = serial.getc();
+ }
+ return true;
+}
+
+
+bool HeptaCamera_GPS::waitRecv()
+{
+ while (!serial.readable()) {
+ }
+ return true;
+}
+
+bool HeptaCamera_GPS::waitIdle()
+{
+ while (serial.readable()) {
+ serial.getc();
+ }
+ return true;
+}
+
+void HeptaCamera_GPS::camera_setting(void)
+{
+ GPS_SW = 0;
+ CAM_SW = 1;
+ serial.setTimeout(1);
+ flushSerialBuffer();
+}
+
+void HeptaCamera_GPS::Sync(void)
+{
+ camera_setting();
+ HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+ printf("synchro setting now\r\n");
+ err = sync();
+ int count=0;
+ while(err) {
+ //printf("count = %d\r\n",count);
+ switch(count) {
+ case 0:
+ printf("Connection of camera and mbed at baudrate 14400\r\n");
+ setmbedBaud(HeptaCamera_GPS::Baud14400);
+
+ break;
+ case 1:
+ printf("Connection of camera and mbed at baudrate 115200\r\n");
+ setmbedBaud(HeptaCamera_GPS::Baud115200);
+ break;
+ default:
+ count=0;
+ }
+ count++;
+ err = sync();
+ printf("synchro setting now\r\n");
+ if(!err) {
+ printf("synchro setting finish\r\n");
+ }
+ /* if(!err) {
+ printf("to 115200\r\n");
+ init(HeptaCamera_GPS::Baud115200, HeptaCamera_GPS::JpegResolution320x240);
+ printf("init err2=%d\r\n", err);
+ setmbedBaud(HeptaCamera_GPS::Baud115200);
+ printf("init err3=%d\r\n", err);
+ }*/
+ }
+}
+
+void HeptaCamera_GPS::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
+{
+ HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+ for (int i = 0; i < CAPTURE_FRAMES; i++) {
+ char fname[64];
+ snprintf(fname, sizeof(fname), "/fs/test.jpg");
+ fp_jpeg = fopen(fname, "w");
+
+ err = getJpegSnapshotPicture();
+
+ if (HeptaCamera_GPS::NoError == err) {
+ printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
+ } else {
+ printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
+ }
+ fclose(fp_jpeg);
+ }
+}
+
+void HeptaCamera_GPS::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
+{
+ HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;
+ for (int i = 0; i < CAPTURE_FRAMES; i++) {
+ char fname[64];
+ snprintf(fname, sizeof(fname), "/fs/test.txt");
+ fp_jpeg = fopen(fname, "w");
+
+ err = getJpegSnapshotPicture_data();
+
+ if (HeptaCamera_GPS::NoError == err) {
+ printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
+ } else {
+ printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
+ }
+ fclose(fp_jpeg);
+ }
+}
+
+void HeptaCamera_GPS::initialize(Baud baud,JpegResolution jr)
+{
+ HeptaCamera_GPS::ErrorNumber err = HeptaCamera_GPS::NoError;//ErrorNumber define
+ err = init(baud, jr);//
+ if (HeptaCamera_GPS::NoError == err) {
+ printf("[ OK ] : Camera::init\r\n");
+ setmbedBaud(baud);
+ } else {
+ printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
+ }
+
+}
+
+//*********************serial*********************//
+void HeptaCamera_GPS::gps_setting(void)
+{
+ CAM_SW = 0;
+ GPS_SW = 1;
+ serial._baud(9600);
+ serial.setTimeout(9999);
+}
+
+
+char HeptaCamera_GPS::getc()
+{
+ //gps_setting();
+ //flushSerialBuffer();
+ c = serial.getc();
+ return c;
+}
+int HeptaCamera_GPS::readable()
+{
+ //gps_setting();
+ i = serial.readable();
+ return i;
+}
+void HeptaCamera_GPS::flushSerialBuffer(void)
+{
+ //gps_setting();
+ ite = 0;
+ while (serial.readable()) {
+ serial.getc();
+ ite++;
+ if(ite==100) {
+ break;
+ };
+ }
+ return;
+}
+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)
+{
+ //flushSerialBuffer();
+ int ite = 0;
+ while(serial.getc()!='$') {
+ ite++;
+ if(ite==10000) break;
+ }
+ for(int i=0; i<5; i++) {
+ msg[i] = serial.getc();
+ }
+ if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
+ for(int j=0; j<6; j++) {
+ if(j==0) {
+ for(int i=5; i<256; i++) {
+ msg[i] = serial.getc();
+ if(msg[i] == '\r') {
+ msg[i] = 0;
+ break;
+ }
+ }
+ } else {
+ for(int i=0; i<256; i++) {
+ msgd[i] = serial.getc();
+ if(msgd[i] == '\r') {
+ break;
+ }
+ }
+ if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
+ break;
+ }
+ }
+ }
+ 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) {
+ if(!(quality)) {
+ //latitude(unit transformation)
+ *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
+ //longitude(unit transformation)
+ *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
+ *serial_check = 0;
+ } else {
+ //latitude(unit transformation)
+ *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
+ //longitude(unit transformation)
+ *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
+ *serial_check = 1;
+ }
+ } else {
+ printf("No Data");
+ *serial_check = 2;
+ }
+ } else {
+ *serial_check = 3;
+ }
+}
+
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
+{
+ //flushSerialBuffer();
+ //gps_setting();
+ //height_1 = 29.4;
+ 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};
+ char hei1[8]= {0x00},hei2[8]= {0x00};
+ int i=0,j=0;
+ while (serial.readable()) {
+ serial.getc();
+ }
+loop:
+ while(serial.getc()!='$') {}
+ for(j=0; j<5; j++) {
+ gps_data[1][j]=serial.getc();
+ }
+ if((gps_data[1][2]==0x47)&(gps_data[1][3]==0x47)&(gps_data[1][4]==0x41)) {
+ for(j=0; j<1; j++) {
+ if(j==0) {
+ i=0;
+ while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
+ //pc.putc(gps_data[j+1][i+5]);
+ i++;
+ }
+ gps_data[j+1][i+5]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ } else {
+ while(serial.getc()!='$') {}
+ i=0;
+ while((gps_data[j+1][i] = serial.getc()) != '\r') {
+ //pc.putc(gps_data[j+1][i]);
+ i++;
+ }
+ gps_data[j+1][i]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ }
+ } else {
+ goto loop;
+ }
+ 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) {
+ //hokui
+ d_hokui=int(hokui/100);
+ m_hokui=(hokui-d_hokui*100);
+ //m_hokui=(hokui-d_hokui*100)/60;
+ g_hokui=d_hokui+(hokui-d_hokui*100)/60;
+ //printf("%f\r\n",hokui);
+ sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+ sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
+ sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
+ sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
+
+ //tokei
+ d_tokei=int(tokei/100);
+ m_tokei=(tokei-d_tokei*100);
+ //m_tokei=(tokei-d_tokei*100)/60;
+ g_tokei=d_tokei+(tokei-d_tokei*100)/60;
+ //printf("%f\r\n",tokei);
+ sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+ sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
+ sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
+ sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
+
+ m_height = int(_altitude);
+ cm_height = int((height_1-m_height)*100 );
+ // printf("%f\r\n",_altitude);
+ sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
+ sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
+
+ lat[0] = gph1[0];
+ lat[1] = gph1[1];
+ lat[2] = gph2[0];
+ lat[3] = gph2[1];
+ lat[4] = gph3[0];
+ lat[5] = gph3[1];
+ lat[6] = gph4[0];
+ lat[7] = gph4[1];
+ log[0] = gpt1[0];
+ log[1] = gpt1[1];
+ log[2] = gpt2[0];
+ log[3] = gpt2[1];
+ log[4] = gpt3[0];
+ log[5] = gpt3[1];
+ log[6] = gpt4[0];
+ log[7] = gpt4[1];
+ height[0] = hei1[0];
+ height[1] = hei1[1];
+ height[2] = hei2[0];
+ height[3] = hei2[1];
+ }
+ *dsize1 = 8;
+ *dsize2 = 4;
+}
+/*
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
+{
+ flushSerialBuffer();
+ //gps_setting();
+ //height_1 = 29.4;
+ 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};
+ char hei1[8]= {0x00},hei2[8]= {0x00};
+ int i=0,j=0;
+ while (serial.readable()) {
+ serial.getc();
+ }
+loop:
+ while(serial.getc()!='$') {}
+ for(j=0; j<5; j++) {
+ gps_data[1][j]=serial.getc();
+ }
+ if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)) {
+ for(j=0; j<1; j++) {
+ if(j==0) {
+ i=0;
+ while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
+ //pc.putc(gps_data[j+1][i+5]);
+ i++;
+ }
+ gps_data[j+1][i+5]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ } else {
+ while(serial.getc()!='$') {}
+ i=0;
+ while((gps_data[j+1][i] = serial.getc()) != '\r') {
+ //pc.putc(gps_data[j+1][i]);
+ i++;
+ }
+ gps_data[j+1][i]='\0';
+ i=0;
+ //pc.printf("\n\r");
+ }
+ }
+ } else {
+ goto loop;
+ }
+ if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1) {
+ //hokui
+ d_hokui=int(hokui/100);
+ m_hokui=(hokui-d_hokui*100);
+ //m_hokui=(hokui-d_hokui*100)/60;
+ g_hokui=d_hokui+(hokui-d_hokui*100)/60;
+ sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+ sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
+ sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
+ sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
+
+ //tokei
+ d_tokei=int(tokei/100);
+ m_tokei=(tokei-d_tokei*100);
+ //m_tokei=(tokei-d_tokei*100)/60;
+ g_tokei=d_tokei+(tokei-d_tokei*100)/60;
+ sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+ sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
+ sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
+ sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
+
+ m_height = int(height_1);
+ cm_height = int((height_1-m_height)*100 );
+ //printf("%d\r\n",cm_height);
+ sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
+ sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
+
+ lat[0] = gph1[0];
+ lat[1] = gph1[1];
+ lat[2] = gph2[0];
+ lat[3] = gph2[1];
+ lat[4] = gph3[0];
+ lat[5] = gph3[1];
+ lat[6] = gph4[0];
+ lat[7] = gph4[1];
+ log[0] = gpt1[0];
+ log[1] = gpt1[1];
+ log[2] = gpt2[0];
+ log[3] = gpt2[1];
+ log[4] = gpt3[0];
+ log[5] = gpt3[1];
+ log[6] = gpt4[0];
+ log[7] = gpt4[1];
+ height[0] = hei1[0];
+ height[1] = hei1[1];
+ height[2] = hei2[0];
+ height[3] = hei2[1];
+ }
+ *dsize1 = 8;
+ *dsize2 = 4;
+}*/
+/*
+void HeptaCamera_GPS::lat_log_sensing_u16(char *lat, char *log, char *height, int *serial_check ,int *dsize1, int *dsize2)
+{
+ int quality=0,stnum=0;
+ char ns='A',ew='B',aunit='m';
+ float time=0.0,latitude=0.0,longitude=0.0,hacu=0.0,altitude=0.0;
+
+ flushSerialBuffer();
+ 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};
+ char hei1[8]= {0x00},hei2[8]= {0x00};
+ int ite = 0;
+ while(serial.getc()!='$') {
+ ite++;
+ if(ite==10000) break;
+ }
+ for(int i=0; i<5; i++) {
+ msg[i] = serial.getc();
+ }
+ if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
+ for(int j=0; j<6; j++) {
+ if(j==0) {
+ for(int i=5; i<256; i++) {
+ msg[i] = serial.getc();
+ if(msg[i] == '\r') {
+ msg[i] = 0;
+ break;
+ }
+ }
+ } else {
+ for(int i=0; i<256; i++) {
+ msgd[i] = serial.getc();
+ if(msgd[i] == '\r') {
+ break;
+ }
+ }
+ if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
+ break;
+ }
+ }
+ }
+ 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) {
+ if(!(quality)) {
+
+ *serial_check = 0;
+ } else {
+
+ *serial_check = 1;
+ }
+ } else {
+ printf("No Data");
+ *serial_check = 2;
+ }
+ } else {
+ *serial_check = 3;
+ }
+ //hokui
+ d_hokui=int(latitude/100);
+ m_hokui=(latitude-d_hokui*100);
+ //m_hokui=(hokui-d_hokui*100)/60;
+ g_hokui=d_hokui+(latitude-d_hokui*100)/60;
+ printf("%f\r\n",latitude);
+ sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
+ sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
+ sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
+ sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
+
+ //tokei
+ d_tokei=int(longitude/100);
+ m_tokei=(longitude-d_tokei*100);
+ //m_tokei=(tokei-d_tokei*100)/60;
+ g_tokei=d_tokei+(longitude-d_tokei*100)/60;
+ printf("%f\r\n",longitude);
+ sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
+ sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
+ sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
+ sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
+
+ m_height = int(altitude);
+ cm_height = int((altitude-m_height)*100 );
+ printf("%f\r\n",altitude);
+ sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
+ sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
+
+ lat[0] = gph1[0];
+ lat[1] = gph1[1];
+ lat[2] = gph2[0];
+ lat[3] = gph2[1];
+ lat[4] = gph3[0];
+ lat[5] = gph3[1];
+ lat[6] = gph4[0];
+ lat[7] = gph4[1];
+ log[0] = gpt1[0];
+ log[1] = gpt1[1];
+ log[2] = gpt2[0];
+ log[3] = gpt2[1];
+ log[4] = gpt3[0];
+ log[5] = gpt3[1];
+ log[6] = gpt4[0];
+ log[7] = gpt4[1];
+ height[0] = hei1[0];
+ height[1] = hei1[1];
+ height[2] = hei2[0];
+ height[3] = hei2[1];
+ *dsize1 = 8;
+ *dsize2 = 4;
+}*/
\ No newline at end of file
diff -r a41100627f55 -r 6d9c33df4c09 HeptaCamera_GPS.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HeptaCamera_GPS.h Wed Aug 23 06:18:42 2017 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "SerialBuffered.h"
+
+#ifndef HEPTA_SERIAL_H
+#define HEPTA_SERIAL_H
+
+class HeptaCamera_GPS
+{
+public:
+
+ enum JpegResolution {
+ JpegResolution80x64 = 0x01, // unofficial
+ JpegResolution160x128 = 0x03, // unofficial
+ JpegResolution320x240 = 0x05, // QVGA
+ JpegResolution640x480 = 0x07 // VGA
+ };
+
+ enum ErrorNumber {
+ NoError = 0x00,
+ UnexpectedReply = 0x04,
+ ParameterError = 0x0b,
+ SendRegisterTimeout = 0x0c,
+ CommandIdError = 0x0d,
+ CommandHeaderError = 0xf0,
+ SetTransferPackageSizeWrong = 0x11
+ };
+
+ enum Baud {
+ Baud460800 = 0x02,
+ Baud230400 = 0x03,
+ Baud115200 = 0x04,
+ Baud57600 = 0x05,
+ Baud28800 = 0x06,
+ Baud14400 = 0x07 // Default.
+ };
+
+ enum ResetType {
+ Nomal = 0x00,
+ High = 0xff
+ };
+
+ HeptaCamera_GPS(PinName tx, PinName rx, PinName CAM, PinName GPS);
+
+ ~HeptaCamera_GPS();
+
+ ErrorNumber sync();
+ ErrorNumber init(Baud baud, JpegResolution jr);
+ ErrorNumber getJpegSnapshotPicture();
+ ErrorNumber getJpegSnapshotPicture_data();
+ void setmbedBaud(Baud baud);
+ void camera_setting(void);
+ void Sync(void);
+ void initialize(Baud baud,JpegResolution jr);
+ void test_jpeg_snapshot_picture(int CAPTURE_FRAMES);
+ void test_jpeg_snapshot_data(int CAPTURE_FRAMES);
+ void jpeg_callback(char *buf, size_t siz);
+ void gps_setting(void);
+ char getc();
+ int readable();
+ void flushSerialBuffer(void);
+ void gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *gps_check);
+ void lat_log_sensing_u16(char *lat, char *log, char * height, int *dsize1, int *dsize2);
+
+private:
+ SerialBuffered serial;
+ DigitalOut CAM_SW;
+ DigitalOut GPS_SW;
+ static const int COMMAND_LENGTH = 6;
+ static const int SYNCMAX = 60;
+ static const int packageSize = 256;
+ //static const int CAPTURE_FRAMES = 3;
+
+ char msg[256],msgd[256];
+ int i,ite,rlock,stn;
+ int _quality,_stnum;
+ float _hacu;
+ float _altitude;
+ char _aunit;
+ int _gps_check;
+
+ char c;
+ char gps_data[7][1000];
+ char ns,ew,statas;
+ float time,hokui,tokei,vel;
+ float g_hokui,g_tokei;
+ float d_hokui,m_hokui,d_tokei,m_tokei;
+ int h_time,m_time,s_time;
+ float height_1;
+ int m_height,cm_height;
+
+ ErrorNumber sendInitial(Baud band, JpegResolution jr);
+ ErrorNumber sendGetPicture(void);
+ ErrorNumber sendSnapshot(void);
+ ErrorNumber sendSetPackageSize(uint16_t packageSize);
+ ErrorNumber sendReset(ResetType specialReset);
+ ErrorNumber recvData(uint32_t *length);
+ ErrorNumber sendSync();
+ ErrorNumber recvSync();
+ ErrorNumber sendAck(uint8_t commandId, uint16_t packageId);
+ ErrorNumber recvAckOrNck();
+
+ bool sendBytes(char *buf, size_t len, int timeout_us = 20000);
+ bool recvBytes(char *buf, size_t len, int timeout_us = 20000);
+ bool waitRecv();
+ bool waitIdle();
+};
+#endif
diff -r a41100627f55 -r 6d9c33df4c09 HeptaSerial.cpp
--- a/HeptaSerial.cpp Sat Aug 05 14:20:51 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,883 +0,0 @@
-#include "HeptaSerial.h"
-static FILE *fp_jpeg;
-#define WAITIDLE waitIdle
-#define SENDFUNC sendBytes
-#define RECVFUNC recvBytes
-#define WAITFUNC waitRecv
-int num=0;
-/**
- * Constructor.
- *
- * @param tx A pin for transmit.
- * @param rx A pin for receive.
- * @param baud Baud rate. (Default is Baud14400.)
- */
-HeptaSerial::HeptaSerial(PinName tx, PinName rx, PinName CAM, PinName GPS) : serial(tx, rx), CAM_SW(CAM), GPS_SW(GPS)
-{
- //serial.baud(baud);
-}
-
-/**
- * Destructor.
- */
-HeptaSerial::~HeptaSerial()
-{
-}
-
-/**
- * Make a sync. for baud rate.
- */
-HeptaSerial::ErrorNumber HeptaSerial::sync()
-{
- for (int i = 0; i < SYNCMAX; i++) {
- if (NoError == sendSync()) {
- if (NoError == recvAckOrNck()) {
- if (NoError == recvSync()) {
- if (NoError == sendAck(0x0D, 0x00)) {
- /*
- * After synchronization, the camera needs a little time for AEC and AGC to be stable.
- * Users should wait for 1-2 seconds before capturing the first picture.
- */
- wait(2.0);
- return NoError;
- }
- }
- }
- }
- wait_ms(50);
- }
- return UnexpectedReply;
-}
-
-/**
- * Initialize.
- *
- * @param baud Camera Interface Speed.
- * @param jr JPEG resolution.
- */
-HeptaSerial::ErrorNumber HeptaSerial::init(Baud baud,JpegResolution jr)
-{
- int i ;
- ErrorNumber en;
- WAITIDLE();
- setmbedBaud((Baud)(0x07)) ;
-
- for ( i = 1 ; i < 7 ; i++ ) {
- if ( NoError == sendSync() ) {
- if ( NoError == recvAckOrNck() ) {
- if ( NoError == recvSync() ) {
- if ( NoError == sendAck(0x0D, 0x00) ) {
- en = sendInitial(baud,jr);
- if (NoError != en) {
- return en;
- }
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
- wait_ms(50) ;
- setmbedBaud(baud);
- //wait_ms(50) ;
- static bool alreadySetupPackageSize = false;
- if (!alreadySetupPackageSize) {
- en = sendSetPackageSize(packageSize);
- if (NoError != en) {
- return en;
- }
- WAITFUNC();
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
- alreadySetupPackageSize = true;
- }
-
- wait(2.0);
- return (ErrorNumber)NoError;
- /*
- * After synchronization, the camera needs a little time for AEC and AGC to be stable.
- * Users should wait for 1-2 seconds before capturing the first picture.
- */
- }
- }
- } else {
- setmbedBaud((Baud)(i+1)) ;
- }
- }
- wait_ms(50);
- }
- return UnexpectedReply;
-}
-
-
-/**
- * Get JPEG snapshot picture.
- *
- * @param func A pointer to a callback function.
- * You can block this function until saving the image datas.
- * @return Status of the error.
- */
-HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture()
-{
- WAITIDLE();
- ErrorNumber en;
-
-
- en = sendSnapshot();
- if (NoError != en) {
- return en;
- }
- WAITFUNC();
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
-
- en = sendGetPicture();
- if (NoError != en) {
- return en;
- }
- WAITFUNC();
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
-
- /*
- * Data : snapshot picture
- */
- uint32_t length = 0;
- WAITFUNC();
- en = recvData(&length);
- if (NoError != en) {
- return en;
- }
- en = sendAck(0x00, 0);
- if (NoError != en) {
- return en;
- }
-
- char databuf[packageSize - 6];
- uint16_t pkg_total = length / (packageSize - 6);
- for (int i = 0; i <= (int)pkg_total; i++) {
- uint16_t checksum = 0;
- // ID.
- char idbuf[2];
- WAITFUNC();
- if (!RECVFUNC(idbuf, sizeof(idbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
- checksum += idbuf[0];
- checksum += idbuf[1];
- uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
- if (id != i) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- // Size of the data.
- char dsbuf[2];
- WAITFUNC();
- if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- // Received the data.
- checksum += dsbuf[0];
- checksum += dsbuf[1];
- uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
- WAITFUNC();
- if (!RECVFUNC(&databuf[0], ds)) {
- return (ErrorNumber)UnexpectedReply;
- }
- for (int j = 0; j < ds; j++) {
- checksum += databuf[j];
- }
-
- // Verify code.
- char vcbuf[2];
- WAITFUNC();
- if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
- uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
- if (vc != (checksum & 0xff)) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- /*
- * Call a call back function.
- * You can block this function while working.
- */
- size_t siz = ds;
- for (int ii = 0; ii < (int)siz; ii++) {
- fprintf(fp_jpeg, "%c", databuf[ii]);
- }
- /*
- * We should wait for camera working before reply a ACK.
- */
- wait_ms(100);
- en = sendAck(0x00, 1 + i);
- if (NoError != en) {
- return en;
- }
- }
-
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::getJpegSnapshotPicture_data()
-{
- WAITIDLE();
- ErrorNumber en;
-
-
- en = sendSnapshot();
- if (NoError != en) {
- return en;
- }
- WAITFUNC();
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
-
- en = sendGetPicture();
- if (NoError != en) {
- return en;
- }
- WAITFUNC();
- en = recvAckOrNck();
- if (NoError != en) {
- return en;
- }
-
- /*
- * Data : snapshot picture
- */
- uint32_t length = 0;
- WAITFUNC();
- en = recvData(&length);
- if (NoError != en) {
- return en;
- }
- en = sendAck(0x00, 0);
- if (NoError != en) {
- return en;
- }
-
- char databuf[packageSize - 6];
- uint16_t pkg_total = length / (packageSize - 6);
- for (int i = 0; i <= (int)pkg_total; i++) {
- uint16_t checksum = 0;
- // ID.
- char idbuf[2];
- WAITFUNC();
- if (!RECVFUNC(idbuf, sizeof(idbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
- checksum += idbuf[0];
- checksum += idbuf[1];
- uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
- if (id != i) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- // Size of the data.
- char dsbuf[2];
- WAITFUNC();
- if (!RECVFUNC(dsbuf, sizeof(dsbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- // Received the data.
- checksum += dsbuf[0];
- checksum += dsbuf[1];
- uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
- WAITFUNC();
- if (!RECVFUNC(&databuf[0], ds)) {
- return (ErrorNumber)UnexpectedReply;
- }
- for (int j = 0; j < ds; j++) {
- checksum += databuf[j];
- }
-
- // Verify code.
- char vcbuf[2];
- WAITFUNC();
- if (!RECVFUNC(vcbuf, sizeof(vcbuf))) {
- return (ErrorNumber)UnexpectedReply;
- }
- uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
- if (vc != (checksum & 0xff)) {
- return (ErrorNumber)UnexpectedReply;
- }
-
- /*
- * Call a call back function.
- * You can block this function while working.
- */
- size_t siz = ds;
- for (int ii = 0; ii < (int)siz; ii++) {
- fprintf(fp_jpeg, "%02X ", databuf[ii]);
- if(++num%16==0) fprintf(fp_jpeg,"\r\n");
- }
- /*
- * We should wait for camera working before reply a ACK.
- */
- wait_ms(100);
- en = sendAck(0x00, 1 + i);
- if (NoError != en) {
- return en;
- }
- }
-
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendInitial(Baud baud, JpegResolution jr)
-{
- char send[COMMAND_LENGTH];
-
- send[0] = 0xAA;
- send[1] = 0x01;
- send[2] = (char)baud;
- send[3] = 0x07;
- send[4] = 0x00;
- send[5] = (char)jr;
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
-
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendGetPicture(void)
-{
- char send[COMMAND_LENGTH];
-
- send[0] = 0xAA;
- send[1] = 0x04;
- send[2] = 0x01;
- send[3] = 0x00;
- send[4] = 0x00;
- send[5] = 0x00;
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSnapshot(void)
-{
- char send[COMMAND_LENGTH];
- send[0] = 0xAA;
- send[1] = 0x05;
- send[2] = 0x00;
- send[3] = 0x00;
- send[4] = 0x00;
- send[5] = 0x00;
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSetPackageSize(uint16_t packageSize)
-{
- char send[COMMAND_LENGTH];
- send[0] = 0xAA;
- send[1] = 0x06;
- send[2] = 0x08;
- send[3] = (packageSize >> 0) & 0xff;
- send[4] = (packageSize >> 8) & 0xff;
- send[5] = 0x00;
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
- return (ErrorNumber)NoError;
-}
-
-void HeptaSerial::setmbedBaud(Baud baud)
-{
- switch((int)baud) {
- case 0x02:
- serial._baud(460800);
- break;
- case 0x03:
- serial._baud(230400);
- break;
- case 0x04:
- serial._baud(115200);
- break;
- case 0x05:
- serial._baud(57600);
- break;
- case 0x06:
- serial._baud((int)28800);
- break;
- case 0x07:
- serial._baud(14400);
- break;
- default:
- serial._baud(14400);
- }
-}
-
-
-HeptaSerial::ErrorNumber HeptaSerial::sendReset(ResetType specialReset)
-{
- char send[COMMAND_LENGTH];
- send[0] = 0xAA;
- send[1] = 0x08;
- send[2] = 0x00;
- send[3] = 0x00;
- send[4] = 0x00;
- send[5] = specialReset;
- /*
- * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
- */
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
-
- return (ErrorNumber)NoError;
-}
-
-
-HeptaSerial::ErrorNumber HeptaSerial::recvData(uint32_t *length)
-{
- char recv[COMMAND_LENGTH];
- if (!RECVFUNC(recv, sizeof(recv))) {
- return (ErrorNumber)UnexpectedReply;
- }
- if ((0xAA != recv[0]) || (0x0A != recv[1])) {
- return (ErrorNumber)UnexpectedReply;
- }
- recv[2] = (char)0x01;
- *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::sendSync()
-{
- char send[COMMAND_LENGTH];
- send[0] = 0xAA;
- send[1] = 0x0D;
- send[2] = 0x00;
- send[3] = 0x00;
- send[4] = 0x00;
- send[5] = 0x00;
-
-
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
- return (ErrorNumber)NoError;
-}
-
-HeptaSerial::ErrorNumber HeptaSerial::recvSync()
-{
- char recv[COMMAND_LENGTH];
-
- if (!RECVFUNC(recv, sizeof(recv))) {
- return (ErrorNumber)UnexpectedReply;
- }
- if ((0xAA != recv[0]) || (0x0D != recv[1])) {
- return (ErrorNumber)UnexpectedReply;
- }
- return (ErrorNumber)NoError;
-}
-
-/**
- * Send ACK.
- *
- * @param commandId The command with that ID is acknowledged by this command.
- * @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.
- */
-HeptaSerial::ErrorNumber HeptaSerial::sendAck(uint8_t commandId, uint16_t packageId)
-{
- char send[COMMAND_LENGTH];
-
- send[0] = 0xAA;
- send[1] = 0x0E;
- send[2] = commandId;
- send[3] = 0x00; // ACK counter is not used.
- send[4] = (packageId >> 0) & 0xff;
- send[5] = (packageId >> 8) & 0xff;
- if (!SENDFUNC(send, sizeof(send))) {
- return (ErrorNumber)SendRegisterTimeout;
- }
- return (ErrorNumber)NoError;
-}
-
-/**
- * Receive ACK or NCK.
- *
- * @return Error number.
- */
-HeptaSerial::ErrorNumber HeptaSerial::recvAckOrNck()
-{
- char recv[COMMAND_LENGTH];
-
- if (!RECVFUNC(recv, sizeof(recv))) {
- return (ErrorNumber)UnexpectedReply;
- }
- if ((0xAA == recv[0]) && (0x0E == recv[1])) {
- return (ErrorNumber)NoError;
- }
- if ((0xAA == recv[0]) && (0x0F == recv[1]) && (0x00 == recv[2])) {
- return (ErrorNumber)NoError;
- }
- if ((0xAA == recv[0]) && (0x0F == recv[1])) {
- return (ErrorNumber)recv[4];
- }
- return (ErrorNumber)UnexpectedReply;
-}
-
-/**
- * Send bytes to camera module.
- *
- * @param buf Pointer to the data buffer.
- * @param len Length of the data buffer.
- *
- * @return True if the data sended.
- */
-bool HeptaSerial::sendBytes(char *buf, size_t len, int timeout_us)
-{
- for (uint32_t i = 0; i < (uint32_t)len; i++) {
- int cnt = 0;
- while (!serial.writeable()) {
- wait_us(1);
- cnt++;
- if (timeout_us < cnt) {
- return false;
- }
- }
- serial.putc(buf[i]);
- }
- return true;
-}
-
-/**
- * Receive bytes from camera module.
- *
- * @param buf Pointer to the data buffer.
- * @param len Length of the data buffer.
- *
- * @return True if the data received.
- */
-bool HeptaSerial::recvBytes(char *buf, size_t len, int timeout_us)
-{
- for (uint32_t i = 0; i < (uint32_t)len; i++) {
- int cnt = 0;
- while (!serial.readable()) {
- wait_us(1);
- cnt++;
- if (timeout_us < cnt) {
- return false;
- }
- }
- buf[i] = serial.getc();
- }
- return true;
-}
-
-/**
- * Wait received.
- *
- * @return True if the data received.
- */
-bool HeptaSerial::waitRecv()
-{
- while (!serial.readable()) {
- }
- return true;
-}
-
-/**
- * Wait idle state.
- */
-bool HeptaSerial::waitIdle()
-{
- while (serial.readable()) {
- serial.getc();
- }
- return true;
-}
-
-void HeptaSerial::camera_setting(void)
-{
- CAM_SW = 1;
- GPS_SW = 0;
- serial.setTimeout(1);
-}
-
-void HeptaSerial::Sync(void)
-{
- camera_setting();
- HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
- err = sync();
- int count=0;
- while(err) {
- printf("count = %d\r\n",count);
- switch(count) {
- case 0:
- setmbedBaud(HeptaSerial::Baud14400);
- break;
- case 1:
- setmbedBaud(HeptaSerial::Baud115200);
- break;
- default:
- count=0;
- }
- count++;
- err = sync();
- printf("init err1=%d\r\n", err);
- if(!err) {
- printf("to 115200\r\n");
- init(HeptaSerial::Baud115200, HeptaSerial::JpegResolution320x240);
- printf("init err2=%d\r\n", err);
- setmbedBaud(HeptaSerial::Baud115200);
- printf("init err3=%d\r\n", err);
- }
- }
-}
-
-void HeptaSerial::test_jpeg_snapshot_picture(int CAPTURE_FRAMES)
-{
- HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
- for (int i = 0; i < CAPTURE_FRAMES; i++) {
- char fname[64];
- snprintf(fname, sizeof(fname), "/fs/test.jpg");
- fp_jpeg = fopen(fname, "w");
-
- err = getJpegSnapshotPicture();
-
- if (HeptaSerial::NoError == err) {
- printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
- } else {
- printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
- }
- fclose(fp_jpeg);
- }
-}
-
-void HeptaSerial::test_jpeg_snapshot_data(int CAPTURE_FRAMES)
-{
- HeptaSerial::ErrorNumber err = HeptaSerial::NoError;
- for (int i = 0; i < CAPTURE_FRAMES; i++) {
- char fname[64];
- snprintf(fname, sizeof(fname), "/fs/test.txt");
- fp_jpeg = fopen(fname, "w");
-
- err = getJpegSnapshotPicture_data();
-
- if (HeptaSerial::NoError == err) {
- printf("[ OK ] : Camera::getJpegSnapshotPicture\r\n");
- } else {
- printf("[FAIL] : Camera::getJpegSnapshotPicture (Error=%02X)\r\n", (int)err);
- }
- fclose(fp_jpeg);
- }
-}
-
-void HeptaSerial::initialize(Baud baud,JpegResolution jr)
-{
- HeptaSerial::ErrorNumber err = HeptaSerial::NoError;//ErrorNumber define
- err = init(baud, jr);//
- if (HeptaSerial::NoError == err) {
- printf("[ OK ] : Camera::init\r\n");
- setmbedBaud(baud);
- } else {
- printf("[FAIL] : Camera::init (Error=%02X)\r\n", (int)err);
- }
-
-}
-
-//*********************serial*********************//
-void HeptaSerial::gps_setting(void)
-{
- CAM_SW = 0;
- GPS_SW = 1;
- serial._baud(9600);
- serial.setTimeout(9999);
-}
-
-
-char HeptaSerial::getc()
-{
- gps_setting();
- c = serial.getc();
- return c;
-}
-int HeptaSerial::readable()
-{
- gps_setting();
- i = serial.readable();
- return i;
-}
-void HeptaSerial::flushSerialBuffer(void)
-{
- gps_setting();
- ite = 0;
- while (serial.readable()) {
- serial.getc();
- ite++;
- if(ite==100) {
- break;
- };
- }
- return;
-}
-void HeptaSerial::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)
-{
- gps_setting();
- int ite = 0;
- while(serial.getc()!='$') {
- ite++;
- if(ite==10000) break;
- }
- for(int i=0; i<5; i++) {
- msg[i] = serial.getc();
- }
- if((msg[2]=='G')&(msg[3]=='G')&(msg[4]=='A')) {
- for(int j=0; j<6; j++) {
- if(j==0) {
- for(int i=5; i<256; i++) {
- msg[i] = serial.getc();
- if(msg[i] == '\r') {
- msg[i] = 0;
- break;
- }
- }
- } else {
- for(int i=0; i<256; i++) {
- msgd[i] = serial.getc();
- if(msgd[i] == '\r') {
- break;
- }
- }
- if((msgd[4]=='V')&(msgd[5]=='T')&(msgd[6]=='G')) {
- break;
- }
- }
- }
- 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) {
- if(!(quality)) {
- //latitude(unit transformation)
- *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
- //longitude(unit transformation)
- *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
- *serial_check = 0;
- } else {
- //latitude(unit transformation)
- *latitude=int(*latitude/100)+(*latitude-int(*latitude/100)*100)/60;
- //longitude(unit transformation)
- *longitude = int(*longitude/100)+(*longitude-int(*longitude/100)*100)/60;
- *serial_check = 1;
- }
- } else {
- printf("No Data");
- *serial_check = 2;
- }
- } else {
- *serial_check = 3;
- }
-}
-void HeptaSerial::lat_log_sensing_u16(char *lat, char *log, char *height, int *dsize1, int *dsize2)
-{
- gps_setting();
- height_1 = 29.4;
- 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};
- char hei1[8]= {0x00},hei2[8]= {0x00};
- int i=0,j=0;
- while (serial.readable()) {
- serial.getc();
- }
-loop:
- while(serial.getc()!='$') {}
- for(j=0; j<5; j++) {
- gps_data[1][j]=serial.getc();
- }
- if((gps_data[1][2]==0x52)&(gps_data[1][3]==0x4d)&(gps_data[1][4]==0x43)) {
- for(j=0; j<1; j++) {
- if(j==0) {
- i=0;
- while((gps_data[j+1][i+5] = serial.getc()) != '\r') {
- //pc.putc(gps_data[j+1][i+5]);
- i++;
- }
- gps_data[j+1][i+5]='\0';
- i=0;
- //pc.printf("\n\r");
- } else {
- while(serial.getc()!='$') {}
- i=0;
- while((gps_data[j+1][i] = serial.getc()) != '\r') {
- //pc.putc(gps_data[j+1][i]);
- i++;
- }
- gps_data[j+1][i]='\0';
- i=0;
- //pc.printf("\n\r");
- }
- }
- } else {
- goto loop;
- }
- if( sscanf(gps_data[1],"GPRMC,%f,%c,%f,%c,%f,%c,%f",&time,&statas,&hokui,&ns,&tokei,&ew,&vel) >= 1) {
- //hokui
- d_hokui=int(hokui/100);
- m_hokui=(hokui-d_hokui*100);
- //m_hokui=(hokui-d_hokui*100)/60;
- g_hokui=d_hokui+(hokui-d_hokui*100)/60;
- sprintf( gph1, "%02X", (char(d_hokui)) & 0xFF);
- sprintf( gph2, "%02X", (char(m_hokui)) & 0xFF);
- sprintf( gph3, "%02X", (char((m_hokui-char(m_hokui))*100)) & 0xFF);
- sprintf( gph4, "%02X", (char(((m_hokui-char(m_hokui))*100-char((m_hokui-char(m_hokui))*100))*100)) & 0xFF);
-
- //tokei
- d_tokei=int(tokei/100);
- m_tokei=(tokei-d_tokei*100);
- //m_tokei=(tokei-d_tokei*100)/60;
- g_tokei=d_tokei+(tokei-d_tokei*100)/60;
- sprintf( gpt1, "%02X", (char(d_tokei)) & 0xFF);
- sprintf( gpt2, "%02X", (char(m_tokei)) & 0xFF);
- sprintf( gpt3, "%02X", (char((m_tokei-char(m_tokei))*100)) & 0xFF);
- sprintf( gpt4, "%02X", (char(((m_tokei-char(m_tokei))*100-char((m_tokei-char(m_tokei))*100))*100)) & 0xFF);
-
- m_height = int(height_1);
- cm_height = int((height_1-m_height)*100 );
- //printf("%d\r\n",cm_height);
- sprintf( hei1, "%02X", (char(m_height)) & 0xFF);
- sprintf( hei2, "%02X", (char(cm_height)) & 0xFF);
-
- lat[0] = gph1[0];
- lat[1] = gph1[1];
- lat[2] = gph2[0];
- lat[3] = gph2[1];
- lat[4] = gph3[0];
- lat[5] = gph3[1];
- lat[6] = gph4[0];
- lat[7] = gph4[1];
- log[0] = gpt1[0];
- log[1] = gpt1[1];
- log[2] = gpt2[0];
- log[3] = gpt2[1];
- log[4] = gpt3[0];
- log[5] = gpt3[1];
- log[6] = gpt4[0];
- log[7] = gpt4[1];
- height[0] = hei1[0];
- height[1] = hei1[1];
- height[2] = hei2[0];
- height[3] = hei2[1];
- }
- *dsize1 = 8;
- *dsize2 = 4;
-}
\ No newline at end of file
diff -r a41100627f55 -r 6d9c33df4c09 HeptaSerial.h
--- a/HeptaSerial.h Sat Aug 05 14:20:51 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,101 +0,0 @@
-#include "mbed.h"
-#include "SerialBuffered.h"
-
-#ifndef HEPTA_SERIAL_H
-#define HEPTA_SERIAL_H
-
-class HeptaSerial
-{
-public:
-
- enum JpegResolution {
- JpegResolution80x64 = 0x01, // unofficial
- JpegResolution160x128 = 0x03, // unofficial
- JpegResolution320x240 = 0x05, // QVGA
- JpegResolution640x480 = 0x07 // VGA
- };
-
- enum ErrorNumber {
- NoError = 0x00,
- UnexpectedReply = 0x04,
- ParameterError = 0x0b,
- SendRegisterTimeout = 0x0c,
- CommandIdError = 0x0d,
- CommandHeaderError = 0xf0,
- SetTransferPackageSizeWrong = 0x11
- };
-
- enum Baud {
- Baud460800 = 0x02,
- Baud230400 = 0x03,
- Baud115200 = 0x04,
- Baud57600 = 0x05,
- Baud28800 = 0x06,
- Baud14400 = 0x07 // Default.
- };
-
- enum ResetType {
- Nomal = 0x00,
- High = 0xff
- };
-
- HeptaSerial(PinName tx, PinName rx, PinName CAM, PinName GPS);
-
- ~HeptaSerial();
-
- ErrorNumber sync();
- ErrorNumber init(Baud baud, JpegResolution jr);
- ErrorNumber getJpegSnapshotPicture();
- ErrorNumber getJpegSnapshotPicture_data();
- void setmbedBaud(Baud baud);
- void camera_setting(void);
- void Sync(void);
- void initialize(Baud baud,JpegResolution jr);
- void test_jpeg_snapshot_picture(int CAPTURE_FRAMES);
- void test_jpeg_snapshot_data(int CAPTURE_FRAMES);
- void jpeg_callback(char *buf, size_t siz);
- void gps_setting(void);
- char getc();
- int readable();
- void flushSerialBuffer(void);
- void gga_sensing(float *time, float *latitude, char *ns, float *longitude, char *ew, int *quality, int *stnum, float *hacu, float *altitude, char *aunit, int *gps_check);
- void lat_log_sensing_u16(char *lat, char *log, char * height, int *dsize1, int *dsize2);
-
-private:
- SerialBuffered serial;
- DigitalOut CAM_SW;
- DigitalOut GPS_SW;
- static const int COMMAND_LENGTH = 6;
- static const int SYNCMAX = 60;
- static const int packageSize = 256;
- //static const int CAPTURE_FRAMES = 3;
-
- char msg[256],msgd[256];
- int i,ite,rlock,stn;
- char c;
- char gps_data[7][1000];
- char ns,ew,statas;
- float time,hokui,tokei,vel;
- float g_hokui,g_tokei;
- float d_hokui,m_hokui,d_tokei,m_tokei;
- int h_time,m_time,s_time;
- float height_1;
- int m_height,cm_height;
-
- ErrorNumber sendInitial(Baud band, JpegResolution jr);
- ErrorNumber sendGetPicture(void);
- ErrorNumber sendSnapshot(void);
- ErrorNumber sendSetPackageSize(uint16_t packageSize);
- ErrorNumber sendReset(ResetType specialReset);
- ErrorNumber recvData(uint32_t *length);
- ErrorNumber sendSync();
- ErrorNumber recvSync();
- ErrorNumber sendAck(uint8_t commandId, uint16_t packageId);
- ErrorNumber recvAckOrNck();
-
- bool sendBytes(char *buf, size_t len, int timeout_us = 20000);
- bool recvBytes(char *buf, size_t len, int timeout_us = 20000);
- bool waitRecv();
- bool waitIdle();
-};
-#endif