Toshihisa T
Skytraq S1315F-RAW-EVK Logger
Revision 0:e0ec137da369, committed 2010-12-18
- Comitter:
- tosihisa
- Date:
- Sat Dec 18 11:17:21 2010 +0000
- Child:
- 1:d1bb695fe3bc
- Commit message:
- 1st release
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FATFileSystem.lib Sat Dec 18 11:17:21 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/fatfilesystem/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.cpp Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,457 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+/* Introduction
+ * ------------
+ * SD and MMC cards support a number of interfaces, but common to them all
+ * is one based on SPI. This is the one I'm implmenting because it means
+ * it is much more portable even though not so performant, and we already
+ * have the mbed SPI Interface!
+ *
+ * The main reference I'm using is Chapter 7, "SPI Mode" of:
+ * http://www.sdcard.org/developers/tech/sdcard/pls/Simplified_Physical_Layer_Spec.pdf
+ *
+ * SPI Startup
+ * -----------
+ * The SD card powers up in SD mode. The SPI interface mode is selected by
+ * asserting CS low and sending the reset command (CMD0). The card will
+ * respond with a (R1) response.
+ *
+ * CMD8 is optionally sent to determine the voltage range supported, and
+ * indirectly determine whether it is a version 1.x SD/non-SD card or
+ * version 2.x. I'll just ignore this for now.
+ *
+ * ACMD41 is repeatedly issued to initialise the card, until "in idle"
+ * (bit 0) of the R1 response goes to '0', indicating it is initialised.
+ *
+ * You should also indicate whether the host supports High Capicity cards,
+ * and check whether the card is high capacity - i'll also ignore this
+ *
+ * SPI Protocol
+ * ------------
+ * The SD SPI protocol is based on transactions made up of 8-bit words, with
+ * the host starting every bus transaction by asserting the CS signal low. The
+ * card always responds to commands, data blocks and errors.
+ *
+ * The protocol supports a CRC, but by default it is off (except for the
+ * first reset CMD0, where the CRC can just be pre-calculated, and CMD8)
+ * I'll leave the CRC off I think!
+ *
+ * Standard capacity cards have variable data block sizes, whereas High
+ * Capacity cards fix the size of data block to 512 bytes. I'll therefore
+ * just always use the Standard Capacity cards with a block size of 512 bytes.
+ * This is set with CMD16.
+ *
+ * You can read and write single blocks (CMD17, CMD25) or multiple blocks
+ * (CMD18, CMD25). For simplicity, I'll just use single block accesses. When
+ * the card gets a read command, it responds with a response token, and then
+ * a data token or an error.
+ *
+ * SPI Command Format
+ * ------------------
+ * Commands are 6-bytes long, containing the command, 32-bit argument, and CRC.
+ *
+ * +---------------+------------+------------+-----------+----------+--------------+
+ * | 01 | cmd[5:0] | arg[31:24] | arg[23:16] | arg[15:8] | arg[7:0] | crc[6:0] | 1 |
+ * +---------------+------------+------------+-----------+----------+--------------+
+ *
+ * As I'm not using CRC, I can fix that byte to what is needed for CMD0 (0x95)
+ *
+ * All Application Specific commands shall be preceded with APP_CMD (CMD55).
+ *
+ * SPI Response Format
+ * -------------------
+ * The main response format (R1) is a status byte (normally zero). Key flags:
+ * idle - 1 if the card is in an idle state/initialising
+ * cmd - 1 if an illegal command code was detected
+ *
+ * +-------------------------------------------------+
+ * R1 | 0 | arg | addr | seq | crc | cmd | erase | idle |
+ * +-------------------------------------------------+
+ *
+ * R1b is the same, except it is followed by a busy signal (zeros) until
+ * the first non-zero byte when it is ready again.
+ *
+ * Data Response Token
+ * -------------------
+ * Every data block written to the card is acknowledged by a byte
+ * response token
+ *
+ * +----------------------+
+ * | xxx | 0 | status | 1 |
+ * +----------------------+
+ * 010 - OK!
+ * 101 - CRC Error
+ * 110 - Write Error
+ *
+ * Single Block Read and Write
+ * ---------------------------
+ *
+ * Block transfers have a byte header, followed by the data, followed
+ * by a 16-bit CRC. In our case, the data will always be 512 bytes.
+ *
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ * | 0xFE | data[0] | data[1] | | data[n] | crc[15:8] | crc[7:0] |
+ * +------+---------+---------+- - - -+---------+-----------+----------+
+ */
+
+#include "SDFileSystem.h"
+
+#define SD_COMMAND_TIMEOUT 5000
+
+SDFileSystem::SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name) :
+ FATFileSystem(name), _spi(mosi, miso, sclk), _cs(cs) {
+ _cs = 1;
+}
+
+#define R1_IDLE_STATE (1 << 0)
+#define R1_ERASE_RESET (1 << 1)
+#define R1_ILLEGAL_COMMAND (1 << 2)
+#define R1_COM_CRC_ERROR (1 << 3)
+#define R1_ERASE_SEQUENCE_ERROR (1 << 4)
+#define R1_ADDRESS_ERROR (1 << 5)
+#define R1_PARAMETER_ERROR (1 << 6)
+
+// Types
+// - v1.x Standard Capacity
+// - v2.x Standard Capacity
+// - v2.x High Capacity
+// - Not recognised as an SD Card
+
+#define SDCARD_FAIL 0
+#define SDCARD_V1 1
+#define SDCARD_V2 2
+#define SDCARD_V2HC 3
+
+int SDFileSystem::initialise_card() {
+ // Set to 100kHz for initialisation, and clock card with cs = 1
+ _spi.frequency(100000);
+ _cs = 1;
+ for(int i=0; i<16; i++) {
+ _spi.write(0xFF);
+ }
+
+ // send CMD0, should return with all zeros except IDLE STATE set (bit 0)
+ if(_cmd(0, 0) != R1_IDLE_STATE) {
+ fprintf(stderr, "No disk, or could not put SD card in to SPI idle state\n");
+ return SDCARD_FAIL;
+ }
+
+ // send CMD8 to determine whther it is ver 2.x
+ int r = _cmd8();
+ if(r == R1_IDLE_STATE) {
+ return initialise_card_v2();
+ } else if(r == (R1_IDLE_STATE | R1_ILLEGAL_COMMAND)) {
+ return initialise_card_v1();
+ } else {
+ fprintf(stderr, "Not in idle state after sending CMD8 (not an SD card?)\n");
+ return SDCARD_FAIL;
+ }
+}
+
+int SDFileSystem::initialise_card_v1() {
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ return SDCARD_V1;
+ }
+ }
+
+ fprintf(stderr, "Timeout waiting for v1.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::initialise_card_v2() {
+
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ _cmd(55, 0);
+ if(_cmd(41, 0) == 0) {
+ _cmd58();
+ return SDCARD_V2;
+ }
+ }
+
+ fprintf(stderr, "Timeout waiting for v2.x card\n");
+ return SDCARD_FAIL;
+}
+
+int SDFileSystem::disk_initialize() {
+
+ int i = initialise_card();
+// printf("init card = %d\n", i);
+// printf("OK\n");
+
+ _sectors = _sd_sectors();
+
+ // Set block length to 512 (CMD16)
+ if(_cmd(16, 512) != 0) {
+ fprintf(stderr, "Set 512-byte block timed out\n");
+ return 1;
+ }
+
+ _spi.frequency(1000000); // Set to 1MHz for data transfer
+ return 0;
+}
+
+int SDFileSystem::disk_write(const char *buffer, int block_number) {
+ // set write address for single block (CMD24)
+ if(_cmd(24, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // send the data block
+ _write(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_read(char *buffer, int block_number) {
+ // set read address for single block (CMD17)
+ if(_cmd(17, block_number * 512) != 0) {
+ return 1;
+ }
+
+ // receive the data
+ _read(buffer, 512);
+ return 0;
+}
+
+int SDFileSystem::disk_status() { return 0; }
+int SDFileSystem::disk_sync() { return 0; }
+int SDFileSystem::disk_sectors() { return _sectors; }
+
+// PRIVATE FUNCTIONS
+
+int SDFileSystem::_cmd(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+int SDFileSystem::_cmdx(int cmd, int arg) {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | cmd);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+
+int SDFileSystem::_cmd58() {
+ _cs = 0;
+ int arg = 0;
+
+ // send a command
+ _spi.write(0x40 | 58);
+ _spi.write(arg >> 24);
+ _spi.write(arg >> 16);
+ _spi.write(arg >> 8);
+ _spi.write(arg >> 0);
+ _spi.write(0x95);
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT; i++) {
+ int response = _spi.write(0xFF);
+ if(!(response & 0x80)) {
+ int ocr = _spi.write(0xFF) << 24;
+ ocr |= _spi.write(0xFF) << 16;
+ ocr |= _spi.write(0xFF) << 8;
+ ocr |= _spi.write(0xFF) << 0;
+// printf("OCR = 0x%08X\n", ocr);
+ _cs = 1;
+ _spi.write(0xFF);
+ return response;
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_cmd8() {
+ _cs = 0;
+
+ // send a command
+ _spi.write(0x40 | 8); // CMD8
+ _spi.write(0x00); // reserved
+ _spi.write(0x00); // reserved
+ _spi.write(0x01); // 3.3v
+ _spi.write(0xAA); // check pattern
+ _spi.write(0x87); // crc
+
+ // wait for the repsonse (response[7] == 0)
+ for(int i=0; i<SD_COMMAND_TIMEOUT * 1000; i++) {
+ char response[5];
+ response[0] = _spi.write(0xFF);
+ if(!(response[0] & 0x80)) {
+ for(int j=1; j<5; j++) {
+ response[i] = _spi.write(0xFF);
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return response[0];
+ }
+ }
+ _cs = 1;
+ _spi.write(0xFF);
+ return -1; // timeout
+}
+
+int SDFileSystem::_read(char *buffer, int length) {
+ _cs = 0;
+
+ // read until start byte (0xFF)
+ while(_spi.write(0xFF) != 0xFE);
+
+ // read data
+ for(int i=0; i<length; i++) {
+ buffer[i] = _spi.write(0xFF);
+ }
+ _spi.write(0xFF); // checksum
+ _spi.write(0xFF);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+int SDFileSystem::_write(const char *buffer, int length) {
+ _cs = 0;
+
+ // indicate start of block
+ _spi.write(0xFE);
+
+ // write the data
+ for(int i=0; i<length; i++) {
+ _spi.write(buffer[i]);
+ }
+
+ // write the checksum
+ _spi.write(0xFF);
+ _spi.write(0xFF);
+
+ // check the repsonse token
+ if((_spi.write(0xFF) & 0x1F) != 0x05) {
+ _cs = 1;
+ _spi.write(0xFF);
+ return 1;
+ }
+
+ // wait for write to finish
+ while(_spi.write(0xFF) == 0);
+
+ _cs = 1;
+ _spi.write(0xFF);
+ return 0;
+}
+
+static int ext_bits(char *data, int msb, int lsb) {
+ int bits = 0;
+ int size = 1 + msb - lsb;
+ for(int i=0; i<size; i++) {
+ int position = lsb + i;
+ int byte = 15 - (position >> 3);
+ int bit = position & 0x7;
+ int value = (data[byte] >> bit) & 1;
+ bits |= value << i;
+ }
+ return bits;
+}
+
+int SDFileSystem::_sd_sectors() {
+
+ // CMD9, Response R2 (R1 byte + 16-byte block read)
+ if(_cmdx(9, 0) != 0) {
+ fprintf(stderr, "Didn't get a response from the disk\n");
+ return 0;
+ }
+
+ char csd[16];
+ if(_read(csd, 16) != 0) {
+ fprintf(stderr, "Couldn't read csd response from disk\n");
+ return 0;
+ }
+
+ // csd_structure : csd[127:126]
+ // c_size : csd[73:62]
+ // c_size_mult : csd[49:47]
+ // read_bl_len : csd[83:80] - the *maximum* read block length
+
+ int csd_structure = ext_bits(csd, 127, 126);
+ int c_size = ext_bits(csd, 73, 62);
+ int c_size_mult = ext_bits(csd, 49, 47);
+ int read_bl_len = ext_bits(csd, 83, 80);
+
+// printf("CSD_STRUCT = %d\n", csd_structure);
+
+ if(csd_structure != 0) {
+ fprintf(stderr, "This disk tastes funny! I only know about type 0 CSD structures\n");
+ return 0;
+ }
+
+ // memory capacity = BLOCKNR * BLOCK_LEN
+ // where
+ // BLOCKNR = (C_SIZE+1) * MULT
+ // MULT = 2^(C_SIZE_MULT+2) (C_SIZE_MULT < 8)
+ // BLOCK_LEN = 2^READ_BL_LEN, (READ_BL_LEN < 12)
+
+ int block_len = 1 << read_bl_len;
+ int mult = 1 << (c_size_mult + 2);
+ int blocknr = (c_size + 1) * mult;
+ int capacity = blocknr * block_len;
+
+ int blocks = capacity / 512;
+
+ return blocks;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.h Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,81 @@
+/* mbed SDFileSystem Library, for providing file access to SD cards
+ * Copyright (c) 2008-2010, sford
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_SDFILESYSTEM_H
+#define MBED_SDFILESYSTEM_H
+
+#include "mbed.h"
+#include "FATFileSystem.h"
+
+/** Access the filesystem on an SD Card using SPI
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "SDFileSystem.h"
+ *
+ * SDFileSystem sd(p5, p6, p7, p12, "sd"); // mosi, miso, sclk, cs
+ *
+ * int main() {
+ * FILE *fp = fopen("/sd/myfile.txt", "w");
+ * fprintf(fp, "Hello World!\n");
+ * fclose(fp);
+ * }
+ */
+class SDFileSystem : public FATFileSystem {
+public:
+
+ /** Create the File System for accessing an SD Card using SPI
+ *
+ * @param mosi SPI mosi pin connected to SD Card
+ * @param miso SPI miso pin conencted to SD Card
+ * @param sclk SPI sclk pin connected to SD Card
+ * @param cs DigitalOut pin used as SD Card chip select
+ * @param name The name used to access the virtual filesystem
+ */
+ SDFileSystem(PinName mosi, PinName miso, PinName sclk, PinName cs, const char* name);
+ virtual int disk_initialize();
+ virtual int disk_write(const char *buffer, int block_number);
+ virtual int disk_read(char *buffer, int block_number);
+ virtual int disk_status();
+ virtual int disk_sync();
+ virtual int disk_sectors();
+
+protected:
+
+ int _cmd(int cmd, int arg);
+ int _cmdx(int cmd, int arg);
+ int _cmd8();
+ int _cmd58();
+ int initialise_card();
+ int initialise_card_v1();
+ int initialise_card_v2();
+
+ int _read(char *buffer, int length);
+ int _write(const char *buffer, int length);
+ int _sd_sectors();
+ int _sectors;
+
+ SPI _spi;
+ DigitalOut _cs;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Sat Dec 18 11:17:21 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/mbed/libT_getAcc.c Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,61 @@
+
+
+extern "C" {
+static int libT_getAccTOmV(unsigned long Val,long *mV);
+int libT_getAcc1dot5G(unsigned long ADval,long *g);
+int libT_getAcc6G(unsigned long ADval,long *g);
+}
+
+
+static int libT_getAccTOmV(unsigned long Val,long *mV)
+{
+ *mV = 0;
+
+ /* AnalogIn = 1mV = 19.85939393939394 */
+ if(Val >= 32767UL){
+ *mV = ((Val - 32767UL) * 100UL) / 1985UL;
+ *mV = 0 - *mV;
+ } else {
+ *mV = ((32767UL - Val) * 100UL) / 1985UL;
+ }
+ return 0;
+}
+
+int libT_getAcc1dot5G(unsigned long ADval,long *g)
+{
+ long mV = 0;
+ libT_getAccTOmV(ADval,&mV); /* A/D value to mV */
+ *g = (long)(mV / 8); /* 0.01g units */
+ return 0;
+}
+
+int libT_getAcc6G(unsigned long ADval,long *g)
+{
+ long mV = 0;
+ libT_getAccTOmV(ADval,&mV); /* A/D value to mV */
+ *g = (long)(mV / 2); /* 0.01g units */
+ return 0;
+}
+
+#if 0
+int main(void)
+{
+ long g;
+
+ libT_getAcc1dot5G(22767UL,&g);
+ if(g >= 0){
+ printf("1.5g g(DOWN)=%ld.%02ld\n",g/100,g%100);
+ } else {
+ g = g * -1;
+ printf("1.5g g(UP)=%ld.%02ld\n",g/100,g%100);
+ }
+ libT_getAcc6G(22767UL,&g);
+ if(g >= 0){
+ printf("6g g(DOWN)=%ld.%02ld\n",g/100,g%100);
+ } else {
+ g = g * -1;
+ printf("6g g(UP)=%ld.%02ld\n",g/100,g%100);
+ }
+ return 0;
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/mbed/tserialbuffer.h Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,49 @@
+
+#ifndef __TSERIALBUFFER_H
+#define __TSERIALBUFFER_H
+
+#include "mbed.h"
+#include "libT/portable/tringbuffer.h"
+#include "libT/portable/tversion.h"
+
+namespace libT {
+
+class tSerialBuffer : public tRingBuffer<unsigned char> , public Serial , public tVersion {
+
+public:
+ tSerialBuffer(PinName _tx, PinName _rx) : Serial(_tx,_rx) , tVersion(0x20100720/* 2010-07-20 */,0x00000001UL){}
+
+ void recvStart(void){
+ Serial::attach(this,&tSerialBuffer::recvHandler,Serial::RxIrq);
+ }
+ void recvStop(void){
+ Serial::attach(0,Serial::RxIrq);
+ }
+
+ int readable(void){
+ return tRingBuffer::readable();
+ }
+
+ int getc(void){
+ unsigned char _c;
+ int retval = -1;
+ if(tRingBuffer::readable()){
+ (void)tRingBuffer::read(&_c);
+ retval = static_cast<int>(_c);
+ }
+ return retval;
+ }
+
+private:
+ void recvHandler(void){
+ if(Serial::readable()){
+ tRingBuffer::write(static_cast<unsigned char>(Serial::getc()));
+ }
+ }
+
+};
+
+};
+
+#endif /* __TSERIALBUFFER_H */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/portable/NMEA_parse.c Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,594 @@
+
+#include "NMEA_parse.h"
+
+#ifndef NULL
+# define NULL ((void *)0)
+#endif
+
+/*
+ nmealib GM-316 FV-M11
+ GGA Global Positioning System Fix Data. O O O
+ GSA GNSS DOP and Active Satellites O O O
+ GSV GNSS Satellites in View O O O
+ RMC Recommended Minimum Navigation Information O O O
+ VTG Course Over Ground and Ground Speed O O O
+ GGL Geographic Position – Latitude / Longitude X O O
+ ZDA Time & Date X O(synchronized to PPS) O
+ */
+
+static unsigned char _libNMEA_atoi(unsigned char c)
+{
+ if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9')))
+ return (c - (unsigned char)('0'));
+ if((c >= (unsigned char)('A')) && (c <= (unsigned char)('F')))
+ return (c - (unsigned char)('A')) + 10;
+ return (c - (unsigned char)('a')) + 10;
+}
+short libNMEA_Parse1Char(unsigned char c,libNMEA_data_t *buffer)
+{
+ register short cnewst = 0;
+
+ /* It does again at the time of beginning after one line is obtained it. */
+ /* It does again after the syntax error at the time of beginning. */
+ /* １行を得た後ならば，再度初めから */
+ /* パースエラーの後も，再度初めから */
+ if((buffer->cjobst >= LIBNMEA_PARSE_COMPLETE) || (buffer->cjobst < LIBNMEA_PARSE_NONE)){
+ buffer->cjobst = LIBNMEA_PARSE_NONE;
+ }
+
+ if(buffer->cjobst == LIBNMEA_PARSE_NONE){ /* Wait '$' */
+ if((c != (unsigned char)('$')) && (c != (unsigned char)('!'))){
+ return buffer->cjobst;
+ }
+ buffer->rawLength = 0;
+ buffer->sum = 0;
+ }
+
+ /* The size check is previously done because it stores it here in the buffer now. */
+ if(buffer->rawLength >= (sizeof(buffer->raw)-2)){
+ buffer->cjobst = -3; /* Buffer size overflow */
+ return buffer->cjobst;
+ }
+
+ buffer->raw[buffer->rawLength] = c;
+ buffer->rawLength += 1;
+ buffer->raw[buffer->rawLength] = (unsigned char)('\0');
+
+ cnewst = -1; /* Syntax error (It initializes it by the default value.) */
+
+ switch(buffer->cjobst){
+ case LIBNMEA_PARSE_NONE:
+ cnewst = 1;
+ break;
+ case 1: /* データ待ち */
+ cnewst = buffer->cjobst;
+ if(c == (unsigned char)('*')){
+ cnewst += 1; /* チェックサム１文字目を得る */
+ } else {
+ buffer->sum = buffer->sum ^ c; /* チェックサム計算 */
+ }
+ break;
+ case 2: /* チェックサム１文字目 */
+ buffer->sumwk = _libNMEA_atoi(c);
+ buffer->sumwk = buffer->sumwk << 4;
+ cnewst = buffer->cjobst + 1; /* チェックサム２文字目を得る */
+ break;
+ case 3: /* チェックサム２文字目 */
+ if((buffer->sumwk |= _libNMEA_atoi(c)) == buffer->sum){
+ cnewst = LIBNMEA_PARSE_SUMOK; /* CR待ち */
+ } else {
+ //printf("SUM:%02x/%02x\n",buffer->sumwk,buffer->sum);
+ cnewst = -2; /* チェックサムエラー */
+ }
+ break;
+ case LIBNMEA_PARSE_SUMOK: /* CR 待ち */
+ if(c == (unsigned char)0x0d){
+ cnewst = buffer->cjobst + 1; /* LF待ち */
+ }
+ break;
+ case LIBNMEA_PARSE_SUMOK+1: /* LF 待ち */
+ if(c == (unsigned char)0x0a){
+ cnewst = LIBNMEA_PARSE_COMPLETE; /* センテンス完了 */
+ }
+ break;
+ }
+
+ buffer->cjobst = cnewst;
+ return buffer->cjobst;
+}
+
+/* 10進数固定 */
+short libNMEA_atol(unsigned char *numstr,long *val,long *length,unsigned char **nextptr)
+{
+ long retval = 0;
+ int i = 0;
+ int mi = 0;
+ unsigned char c;
+
+ *length = 1;
+
+ if(*(numstr + i) == (unsigned char)('-')){
+ mi = 1;
+ i++;
+ }
+
+ for(;*(numstr + i) != (unsigned char)('\0');i++){
+ c = *(numstr + i);
+ if((c >= (unsigned char)('0')) && (c <= (unsigned char)('9'))){
+ c = c - (unsigned char)('0');
+ retval = (retval * 10) + c;
+ *length = *length * 10;
+ } else {
+ break;
+ }
+ }
+ if((nextptr != NULL) && (*nextptr != NULL)){
+ *nextptr = numstr + i;
+ }
+ *val = (mi) ? (0-retval) : retval;
+ return 0;
+}
+
+short libNMEA_atod(unsigned char *numstr,libNMEA_realnum_t *val,unsigned char **nextptr)
+{
+ long lval = 0;
+ libNMEA_realnum_t e = 0.0;
+ libNMEA_realnum_t f = 0.0;
+ unsigned char *nptr;
+ long length;
+
+ /* TODO:符号チェックを入れる事 */
+
+ /* 整数部 */
+ libNMEA_atol(numstr,&lval,&length,&nptr);
+ e = (libNMEA_realnum_t)lval;
+
+ /* 小数部 */
+ if(*nptr == (unsigned char)('.')){
+ nptr++;
+ libNMEA_atol(nptr,&lval,&length,&nptr);
+ f = ((libNMEA_realnum_t)lval) / ((libNMEA_realnum_t)length);
+ }
+
+ if((nextptr != NULL) && (*nextptr != NULL)){
+ *nextptr = nptr;
+ }
+ *val = e + f;
+ return 0;
+}
+
+/* GPS の緯度経度 ddmm.mmmm(dddmm.mmmm) 形式の文字列を，
+ dd.dddd の libNMEA_realnum_t 形式に変換する． */
+/* http://www.circuitcellar.com/library/print/1000/Stefan123/4.htm */
+short libNMEA_atodeg(unsigned char *numstr,libNMEA_realnum_t *val)
+{
+ volatile long wk = 0;
+ volatile long deg_l = 0;
+ long length = 0;
+ libNMEA_realnum_t mm_d = 0;
+ unsigned char *nextptr;
+
+ /* ddmm 部を取り出す */
+ libNMEA_atol(numstr,(long *)&wk,&length,&nextptr);
+
+ /* dd を得る */
+ deg_l = wk / 100;
+
+ /* .mmmm 部を取り出す */
+ libNMEA_atod(nextptr,&mm_d,&nextptr);
+
+ /* mm.mmmm にする */
+ mm_d = ((libNMEA_realnum_t)(wk % 100)) + mm_d;
+ /* 0.dddd に変換する */
+ mm_d = (mm_d / 60.0);
+ /* もし，mm_d が 1.0 よりも大きい場合，mm.mmmm は60分を超える値であり，
+ GPS の緯度経度値として適切ではない */
+ if(mm_d > 1.0){
+ return -1;
+ }
+
+ *val = ((libNMEA_realnum_t)deg_l) + mm_d; /* dd.dddd 形式に変換 */
+
+ return 0;
+}
+
+#if 0
+static short _libNMEA_print_args(libNMEA_data_t *buffer)
+{
+ int i;
+ extern int printf(const char *format, ...);
+
+ printf("ARGC %5d:",buffer->argc);
+ for(i=0;i < buffer->argc;i++){
+ printf("[%s]",buffer->raw + buffer->argv[i]);
+ }
+ printf("\n");
+ return 0;
+}
+#endif
+
+static void _libNMEA_String2Time(unsigned char *numstr,libNMEA_gps_info_t *GPSinfo)
+{
+ long wk;
+ long length;
+ unsigned char *nextptr;
+
+ libNMEA_atol(numstr,&wk,&length,&nextptr);
+ GPSinfo->time.sec = wk % 100;
+ wk = wk/100;
+ GPSinfo->time.min = wk % 100;
+ wk = wk/100;
+ GPSinfo->time.hour = wk % 100;
+ GPSinfo->time.msec = 0;
+ if((nextptr != NULL) && (*nextptr == (unsigned char)('.'))){
+ libNMEA_atol(nextptr+1,&wk,&length,(unsigned char **)NULL);
+ GPSinfo->time.msec = wk;
+ }
+}
+
+static void _libNMEA_setInvalid(libNMEA_gps_info_t *GPSinfo)
+{
+ GPSinfo->valid.status = 0;
+
+ GPSinfo->valid.positioning = 0;
+ GPSinfo->valid.active = 0;
+}
+
+/* Recommended Minimum Course Response Message */
+static short _libNMEA_Parse1Line_GPRMC(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
+{
+ unsigned char *nextptr;
+
+ if(buffer->argc != 13){
+ return -2; /* データの数がおかしい */
+ }
+ //_libNMEA_print_args(buffer);
+
+ /* まず，真っ先に見るのは，GPS データのステータス(妥当性) */
+ if(*(buffer->raw+buffer->argv[2]+0) != (unsigned char)('A')){
+ /* GPS データ妥当性無し */
+ GPSinfo->valid.status = 0;
+ /* 妥当性が無いので，これ以上解析する必要は無いので終わる */
+ return 0;
+ }
+
+ /* 測位状態を得る */
+ switch(*(buffer->raw+buffer->argv[12]+0)){
+ case (unsigned char)('A'): /* 単独測位 */
+ case (unsigned char)('D'): /* DGPS */
+ GPSinfo->status.positioning = *(buffer->raw+buffer->argv[12]+0);
+ GPSinfo->valid.positioning = 1;
+ break;
+ default:
+ GPSinfo->status.positioning = (unsigned char)('N');
+ _libNMEA_setInvalid(GPSinfo);
+ return 0; /* 無効な測位状態なので返る */
+ }
+
+ /* *** 妥当性のあるデータ *** */
+
+ /* 緯度を得る */
+ libNMEA_atodeg( buffer->raw+buffer->argv[3]
+ ,&(GPSinfo->latlon.latitude) );
+ GPSinfo->latlon.lat_unit = *(buffer->raw+buffer->argv[4]);
+ GPSinfo->valid.latitude = 1;
+
+ /* 経度を得る */
+ libNMEA_atodeg( buffer->raw+buffer->argv[5]
+ ,&(GPSinfo->latlon.longitude ) );
+ GPSinfo->latlon.lon_unit = *(buffer->raw+buffer->argv[6]);
+ GPSinfo->valid.longitude = 1;
+
+ /* 対地速度（ノット）を得る */
+ libNMEA_atod( buffer->raw+buffer->argv[7],
+ &(GPSinfo->speed.ground),
+ &nextptr);
+ GPSinfo->valid.speed = 1;
+
+ /* 進行方向（度，真北）を得る */
+ libNMEA_atod( buffer->raw+buffer->argv[8],
+ &(GPSinfo->direction.trueNorth.deg),
+ &nextptr);
+ GPSinfo->valid.direction = 1;
+
+ GPSinfo->valid.timeRMC = 0;
+ if(1){
+ /* 現在時刻／日付を得る．*/
+ long wk;
+ long length;
+
+ _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo);
+
+ libNMEA_atol(buffer->raw+buffer->argv[9],&wk,&length,&nextptr);
+ /* GPRMC は，西暦は下2桁しかないので，2000を加算する． */
+ GPSinfo->date.year = (wk % 100) + 2000; /* after year 2000. */
+ wk = wk/100;
+ GPSinfo->date.mon = wk % 100;
+ wk = wk/100;
+ GPSinfo->date.day = wk % 100;
+ GPSinfo->valid.timeRMC = 1;
+ }
+
+ /* ステータスは有効 */
+ GPSinfo->valid.status = 1;
+
+ return 0;
+}
+
+static short _libNMEA_Parse1Line_GPZDA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
+{
+ long wk;
+ long length;
+ unsigned char *nextptr;
+
+ if(buffer->argc != 7){
+ return -2;
+ }
+
+ GPSinfo->valid.timeZDA = 0;
+
+ _libNMEA_String2Time(buffer->raw+buffer->argv[1],GPSinfo);
+
+ libNMEA_atol(buffer->raw+buffer->argv[2],&wk,&length,&nextptr);
+ GPSinfo->date.day = wk;
+ libNMEA_atol(buffer->raw+buffer->argv[3],&wk,&length,&nextptr);
+ GPSinfo->date.mon = wk;
+ libNMEA_atol(buffer->raw+buffer->argv[4],&wk,&length,&nextptr);
+ GPSinfo->date.year = wk;
+
+ GPSinfo->valid.timeZDA = 1;
+
+ return 0;
+}
+
+/* GPGSA - GNSS DOP and Active Satellites */
+/* 0:[$GPGSA],1:[A],2:[3],3:[05],4:[02],5:[26],6:[27],7:[09],8:[04],9:[15],10:[],11:[],12:[],13:[],14:[],15:[1.8],16:[1.0],17:[1.5]*11 */
+static short _libNMEA_Parse1Line_GPGSA(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
+{
+ if(buffer->argc != 18){
+ return -2;
+ }
+
+ switch( *(buffer->raw+buffer->argv[2]) ){
+ case '2': /* 2D */
+ case '3': /* 3D */
+ GPSinfo->status.active = *(buffer->raw+buffer->argv[2]);
+ GPSinfo->valid.active = 1;
+ break;
+ default: /* Fix not available */
+ _libNMEA_setInvalid(GPSinfo);
+ return 0; /* 無効な測位状態なので返る */
+ }
+
+ GPSinfo->valid.accuracy_pdop = 0;
+ libNMEA_atod( buffer->raw+buffer->argv[15],
+ &(GPSinfo->accuracy.p.dop),
+ (unsigned char **)NULL);
+ GPSinfo->valid.accuracy_pdop = 1;
+
+ GPSinfo->valid.accuracy_hdop = 0;
+ libNMEA_atod( buffer->raw+buffer->argv[16],
+ &(GPSinfo->accuracy.h.dop),
+ (unsigned char **)NULL);
+ GPSinfo->valid.accuracy_hdop = 1;
+
+ GPSinfo->valid.accuracy_vdop = 0;
+ libNMEA_atod( buffer->raw+buffer->argv[17],
+ &(GPSinfo->accuracy.v.dop),
+ (unsigned char **)NULL);
+ GPSinfo->valid.accuracy_vdop = 1;
+
+ return 0;
+}
+
+static short _libNMEA_Parse1Line_GP(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
+{
+ unsigned char *sentenceID = (unsigned char *)0;
+
+ sentenceID = buffer->raw+buffer->argv[0];
+ sentenceID += 3; /* "$GP" をスキップ */
+
+ if( (*(sentenceID+0) == (unsigned char)('R'))
+ && (*(sentenceID+1) == (unsigned char)('M'))
+ && (*(sentenceID+2) == (unsigned char)('C'))){
+ return _libNMEA_Parse1Line_GPRMC(buffer,GPSinfo);
+ } else if( (*(sentenceID+0) == (unsigned char)('Z'))
+ && (*(sentenceID+1) == (unsigned char)('D'))
+ && (*(sentenceID+2) == (unsigned char)('A'))){
+ return _libNMEA_Parse1Line_GPZDA(buffer,GPSinfo);
+ } else if((*(sentenceID+0) == (unsigned char)('G'))
+ && (*(sentenceID+1) == (unsigned char)('S'))
+ && (*(sentenceID+2) == (unsigned char)('A'))){
+ return _libNMEA_Parse1Line_GPGSA(buffer,GPSinfo);
+ }
+
+ return -2; /* このセンテンスは処理しませんでした */
+}
+
+#ifdef DEBUG /* { */
+static short max_argc = 0;
+#endif /* } */
+
+short libNMEA_Parse1Line(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo)
+{
+ int i;
+ unsigned char *tokerID = (unsigned char *)0;
+
+ buffer->argc = 0;
+ buffer->argv[buffer->argc+0] = 0;
+ buffer->argv[buffer->argc+1] = -1;
+ buffer->argc++;
+
+ /* まず，NMEA センテンスをトークンで分解する．*/
+ /* 後で変換作業をしやすくするため */
+ for(i = 0;buffer->raw[i] != (unsigned char)('\0');i++){
+ if((buffer->raw[i] == (unsigned char)('*'))
+ || (buffer->raw[i] == (unsigned char)(0x0d))
+ || (buffer->raw[i] == (unsigned char)(0x0a))){
+ buffer->raw[i] = (unsigned char)('\0');
+ buffer->argv[buffer->argc] = -1;
+ break;
+ }
+ if(buffer->raw[i] == (unsigned char)(',')){
+ buffer->raw[i] = (unsigned char)('\0');
+ if(buffer->raw[i+1] != (unsigned char)('\0')){
+ buffer->argv[buffer->argc+0] = i+1;
+ buffer->argv[buffer->argc+1] = -1;
+ buffer->argc++;
+ if(buffer->argc >= LIBNMEA_NUMSOFARRAY(buffer->argv)){
+ return -1;
+ }
+#ifdef DEBUG
+ if(buffer->argc > max_argc){
+ max_argc = buffer->argc;
+ }
+#endif
+ }
+ }
+ }
+
+ tokerID = buffer->raw+buffer->argv[0];
+ if(*(tokerID+0) == (unsigned char)('$')){
+ if(*(tokerID+1) == (unsigned char)('P')){
+ /* P センテンスは無視する */
+ } else {
+ if((*(tokerID+1) == (unsigned char)('G'))
+ || (*(tokerID+2) == (unsigned char)('P'))){
+ /* GP for a GPS unit */
+ return _libNMEA_Parse1Line_GP(buffer,GPSinfo);
+ } else {
+ /* 不明なトーカーは無視する */
+ }
+ }
+ } else {
+ /* '$' から始まらないセンテンスは無視する */
+ }
+
+ return -2; /* このセンテンスは処理しませんでした */
+}
+
+#ifdef __SELFTEST__
+#include <stdio.h>
+
+char *sample[] = {
+ "$GPGGA,123519.00,4807.038247,N,01131.324523,E,1,08,0.9,545.42,M,46.93,M,5.0,1012*42\x0d\x0a",
+ "$GPGLL,4916.452349,N,12311.123215,W,225444.00,A,A*6A\x0d\x0a",
+ "$GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39\x0d\x0a",
+ "$GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75\x0d\x0a",
+ "$GPRMC,225446.00,A,4916.452653,N,12311.123747,W,000.5,054.7,191194,06.2,W,A*68\x0d\x0a",
+ NULL
+};
+
+int main(int argc,char *argv[])
+{
+ int i,j;
+ libNMEA_data_t sts;
+ int ret;
+ libNMEA_gps_info_t GPSinfo;
+ unsigned char *nextptr;
+ unsigned char *text;
+ libNMEA_realnum_t val;
+
+
+ /* x86 版 Linux だと，sizeof(libNMEA_gps_info_t) = 124 である．*/
+ printf("sizeof(libNMEA_gps_info_t) = %lu\n\n",sizeof(libNMEA_gps_info_t));
+ printf("sizeof(libNMEA_gps_time_t) = %lu\n\n",sizeof(libNMEA_gps_time_t));
+
+ text = "123.4567"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "123.4567ABC"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "9876"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "98765ABC"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "98765.123"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "98765.00123"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "1.0000123DEF"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "1.000987XYZDEF"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = "12."; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ text = ".9876"; libNMEA_atod(text,&val,&nextptr);
+ printf("test=[%-20s]/val=%20.10f/ptr=[%s]\n",text,val,nextptr);
+ if(libNMEA_atodeg(text,&val) == 0){ printf("dd.dddd[%10.7f]\n\n",val); } else {printf("ERROR\n\n"); }
+
+ for(i = 0;sample[i] != NULL;i++){
+ sts.cjobst = 0;
+ for(j = 0;*(sample[i]+j) != (char)('\0');j++){
+ ret = libNMEA_Parse1Char(*(sample[i]+j),&sts);
+ if(ret < 0){
+ printf("ret = %d (char:\'%c\')\n",ret,*(sample[i]+j));
+ }
+ if(ret == LIBNMEA_PARSE_COMPLETE){
+ printf("mes:[%s]\n",sample[i]);
+ printf("get:[%s]\n",sts.raw);
+ libNMEA_Parse1Line(&sts,&GPSinfo);
+ }
+ }
+ }
+ return 0;
+}
+#endif
+
+#ifdef __SELFTEST2__
+#include <stdio.h>
+#include <unistd.h>
+
+int main(int argc,char *argv[])
+{
+ libNMEA_data_t sts;
+ int ret;
+ char buf;
+ libNMEA_gps_info_t GPSinfo;
+
+ sts.cjobst = 0;
+ while(read(fileno(stdin),&buf,sizeof(buf)) == sizeof(buf)){
+ ret = libNMEA_Parse1Char(buf,&sts);
+ if(ret < 0){
+ printf("ret = %d (char:\'%c\')\n",ret,buf);
+ printf("err:[%s]\n",sts.raw);
+ return 1;
+ }
+ if(ret == LIBNMEA_PARSE_SUMOK){
+ //printf("get:[%s]\n",sts.raw);
+GPSinfo.valid.status = 0;
+ libNMEA_Parse1Line(&sts,&GPSinfo);
+ if(GPSinfo.valid.status != 0){
+ printf("lat:%c %8.5f\nlon:%c %8.5f\n",
+ GPSinfo.latlon.lat_unit,
+ GPSinfo.latlon.latitude,
+ GPSinfo.latlon.lon_unit,
+ GPSinfo.latlon.longitude );
+ } else {
+ //printf("lat:- ---.-----\nlon:- ---.-----\n");
+ }
+
+ sts.cjobst = 0;
+ }
+ }
+ printf("max_argc = %d\n",max_argc);
+ return 0;
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/portable/NMEA_parse.h Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,119 @@
+
+#ifndef _NMEA_PARSE_H
+#define _NMEA_PARSE_H
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define LIBNMEA_PARSE_NONE (0)
+#define LIBNMEA_PARSE_SUMOK (10)
+#define LIBNMEA_PARSE_COMPLETE (100)
+
+#define LIBNMEA_NUMSOFARRAY(array) (sizeof((array))/sizeof((array)[0]))
+
+typedef double libNMEA_realnum_t;
+typedef long libNMEA_intnum_t;
+
+typedef struct {
+// unsigned char isValid;
+ unsigned char active; /* '0' or '1' = Fix not available,'2' = 2D,'3' = 3D */
+ unsigned char positioning; /* 'A' = 単独測位，D = DGPS，N = 無効 */
+} libNMEA_gps_status_t;
+
+typedef struct {
+ unsigned long res1:1;
+ unsigned long year:12;
+ unsigned long mon:4;
+ unsigned long day:6;
+ unsigned long res2:9;
+} libNMEA_gps_date_t;
+
+typedef struct {
+ unsigned long res:1;
+ unsigned long hour:5;
+ unsigned long min:6;
+ unsigned long sec:6;
+ unsigned long msec:14;
+} libNMEA_gps_time_t;
+
+typedef struct {
+// unsigned char isValid;
+ unsigned char unit;
+ libNMEA_realnum_t deg;
+} libNMEA_gps_deg_t;
+
+typedef struct {
+ unsigned char lat_unit; /* 'N'(Nouth) or 'S'(South) */
+ unsigned char lon_unit; /* 'W'(West) or 'E'(East) */
+ libNMEA_realnum_t latitude;
+ libNMEA_realnum_t longitude;
+} libNMEA_gps_latlon_t;
+
+typedef struct {
+// unsigned char isValid;
+ libNMEA_realnum_t ground;
+} libNMEA_gps_speed_t;
+
+typedef struct {
+// unsigned char isValid;
+ libNMEA_realnum_t dop;
+} libNMEA_gps_dop_t;
+
+typedef struct {
+ libNMEA_gps_deg_t trueNorth;
+ libNMEA_gps_deg_t magnetic;
+ libNMEA_gps_deg_t mag_dec;
+} libNMEA_gps_direction_t;
+
+typedef struct {
+ libNMEA_gps_dop_t h;
+ libNMEA_gps_dop_t v;
+ libNMEA_gps_dop_t p;
+} libNMEA_gps_accuracy_t;
+
+typedef struct {
+ unsigned long status:1; /* これが１ならば，GPS のデータを信用する */
+ unsigned long active:1;
+ unsigned long positioning:1;
+ unsigned long date:1;
+ unsigned long timeRMC:1;
+ unsigned long timeZDA:1;
+ unsigned long latitude:1;
+ unsigned long longitude:1;
+ unsigned long speed:1;
+ unsigned long direction:1;
+ unsigned long accuracy_hdop:1;
+ unsigned long accuracy_vdop:1;
+ unsigned long accuracy_pdop:1;
+ unsigned long res:19;
+} libNMEA_gps_valid_t;
+
+typedef struct {
+ libNMEA_gps_valid_t valid;
+ libNMEA_gps_status_t status;
+ libNMEA_gps_date_t date;
+ libNMEA_gps_time_t time;
+ libNMEA_gps_latlon_t latlon;
+ libNMEA_gps_speed_t speed;
+ libNMEA_gps_direction_t direction;
+ libNMEA_gps_accuracy_t accuracy;
+} libNMEA_gps_info_t;
+
+typedef struct {
+ short cjobst;
+ unsigned char raw[82+2];
+ short rawLength;
+ unsigned char sum;
+ unsigned char sumwk;
+ short argc;
+ short argv[31]; /* NMEA センテンスで処理できる最大トークン数．メモリを節約したいのでポインタではなく先頭からのオフセットにしている． */
+} libNMEA_data_t;
+
+extern short libNMEA_Parse1Char(unsigned char c,libNMEA_data_t *buffer);
+short libNMEA_Parse1Line(libNMEA_data_t *buffer,libNMEA_gps_info_t *GPSinfo);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _NMEA_PARSE_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/portable/tringbuffer.h Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,64 @@
+
+#ifndef __TRINGBUFFER_H
+#define __TRINGBUFFER_H
+
+#include "tversion.h"
+
+namespace libT {
+
+template <class T>
+class tRingBuffer : public tVersion
+{
+public:
+
+ tRingBuffer(void) : tVersion(0x20100721/* 2010-07-21 */,0x00000001UL),wp(0),rp(0) {}
+
+ inline void reset(void){wp = rp = 0;}
+
+ inline int writable(void) const
+ {
+ return (inc(wp) != rp) ? 1 : 0;
+ }
+
+ inline int write(T _c)
+ {
+ int retval = -1; /* Ringbuffer FULL */
+ if(writable()){
+ array[wp] = _c;
+ wp = inc(wp);
+ retval = 0; /* OK */
+ }
+ return retval;
+ }
+
+ inline int readable(void) const
+ {
+ return (rp != wp) ? 1 : 0;
+ }
+
+ inline int read(T *_c)
+ {
+ int retval = -1; /* Ringbuffer empty or _c is invalid*/
+ if((readable()) && (_c != 0)){
+ *_c = array[rp];
+ rp = inc(rp);
+ retval = 0; /* OK */
+ }
+ return retval;
+ }
+
+private:
+ enum {TRINGBUFFER_SIZE=0x200};
+ inline unsigned short inc(unsigned short _p) const
+ {
+ return static_cast<unsigned short>(static_cast<unsigned short>(_p + 1) & (sizeof(array)-1));
+ }
+ volatile unsigned short wp;
+ volatile unsigned short rp;
+ volatile T array[TRINGBUFFER_SIZE];
+};
+
+};
+
+#endif /* __TRINGBUFFER_H */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/libT/portable/tversion.h Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,21 @@
+
+#ifndef __TVERSION_H
+#define __TVERSION_H
+
+namespace libT {
+
+class tVersion {
+public:
+ tVersion(unsigned long _major,unsigned long _minor) : major(_major),minor(minor) {}
+
+ unsigned long getMajorVersion(void) const { return major; }
+ unsigned long getMinorVersion(void) const { return minor; }
+
+private:
+ unsigned long major;
+ unsigned long minor;
+};
+
+};
+
+#endif /* __TVERSION_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Dec 18 11:17:21 2010 +0000
@@ -0,0 +1,324 @@
+
+/*
+ * S1315F-RAW-EVK Logger
+ * (c) @tosihisa http://twitter.com/tosihisa
+ */
+
+#include "mbed.h"
+#include "TextLCD.h"
+#include "SDFileSystem.h"
+#include "libT/mbed/tserialbuffer.h"
+
+#define OUTPUT_RATE_1HZ
+
+using namespace libT;
+
+Serial debug(USBTX,USBRX);
+InterruptIn PPS(p15);
+PwmOut PPSLED(LED1);
+PwmOut LOGLED(LED4);
+tSerialBuffer gps(p13,p14);
+DigitalIn LogSW(p22);
+Ticker timer;
+
+TextLCD lcd(p24, p26, p27, p28, p29, p30); // rs, e, d4-d7
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+
+int logSW_state[2] = { 0 , 0 };
+int logSW_idx = 0;
+unsigned long jif = 0;
+void timer_handler()
+{
+ logSW_state[logSW_idx] = (LogSW != 0) ? 1 : 0;
+ logSW_idx = (logSW_idx + 1) & 1;
+ jif++;
+}
+
+int pps_count = 0;
+void pps_rise()
+{
+ pps_count++;
+ if(pps_count & 1)
+ PPSLED=0.20;
+ else
+ PPSLED=0.0;
+}
+
+typedef struct Skytraq_bin_info_s {
+ int cjobst;
+ unsigned short len;
+ unsigned short recv;
+ unsigned char sum;
+ unsigned char ID;
+ unsigned char body[2048];
+ void *work;
+
+ unsigned long KERNEL_ver;
+ unsigned long ODM_ver;
+ unsigned long Revision;
+} Skytraq_bin_info_t;
+
+typedef struct Skytraq_ID_s {
+ unsigned char ID;
+ int (*func)(Skytraq_bin_info_t *info);
+ char *name;
+} Skytraq_ID_t;
+
+int Skytraq_0x80(Skytraq_bin_info_t *info)
+{
+ int idx = 1;
+
+ info->KERNEL_ver = 0;
+ info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
+ info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
+ info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
+ info->KERNEL_ver = (info->KERNEL_ver << 8) | info->body[idx]; idx++;
+
+ info->ODM_ver = 0;
+ info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
+ info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
+ info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
+ info->ODM_ver = (info->ODM_ver << 8) | info->body[idx]; idx++;
+
+ info->Revision = 0;
+ info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
+ info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
+ info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
+ info->Revision = (info->Revision << 8) | info->body[idx]; idx++;
+
+ for(idx = 0;idx < 14;idx++){
+ debug.printf("%02x ",info->body[idx]);
+ }
+ debug.printf("\n");
+ debug.printf("KERNEL:0x%08x , ODM:0x%08x , Rev:0x%08x\n",info->KERNEL_ver,info->ODM_ver,info->Revision);
+
+ return 0;
+}
+
+Skytraq_ID_t Skytraq_ID_List[] = {
+ { 0x01 ,NULL,"Input System Restart Force system to restart"},
+ { 0x02 ,NULL,"Input Query Software version Query revision information of software"},
+ { 0x03 ,NULL,"Input Query Software CRC Query the CRC of the software"},
+ { 0x04 ,NULL,"Input Set Factory Defaults Set system to factory default values"},
+ { 0x05 ,NULL,"Input Configure Serial Port Set up serial port COM, baud rate, data bits, stop bits and parity"},
+ { 0x06 ,NULL,"Input Reserved Reserved"},
+ { 0x07 ,NULL,"Input Reserved Reserved"},
+ { 0x08 ,NULL,"Input Configure NMEA Configure NMEA output message"},
+ { 0x09 ,NULL,"Input Configure Output Message Format Configure the output message format from GPS receiver" },
+ { 0x0C ,NULL,"Input Configure Power Mode Set system power mode" },
+ { 0x0E ,NULL,"Input Configure position update rate Configure the position update rate of GPS system" },
+ { 0x10 ,NULL,"Input Query position update rate Query the position update rate of GPS system" },
+ { 0x11 ,NULL,"Input Get Almanac Retrieve almanac data of the GPS receiver" },
+ { 0x12 ,NULL,"Input Configure binary measurement output rates Configure the output rates of the binary measurement outputs Input GPS Messages" },
+ { 0x29 ,NULL,"Input Configure Datum Configure Datum of the GPS receiver" },
+ { 0x2D ,NULL,"Input Query Datum Query datum used by the GPS receiver" },
+ { 0x30 ,NULL,"Input Get Ephemeris Retrieve ephemeris data of the GPS receiver Output System Messages" },
+ { 0x31 ,NULL,"Input Set ephemeris Set ephemeris data to the GPS receiver" },
+ { 0x37 ,NULL,"Input Configure WAAS Configure the enable or disable of WAAS" },
+ { 0x38 ,NULL,"Input Query WAAS status Query WAAS status of GPS receiver" },
+ { 0x39 ,NULL,"Input Configure position pinning Enable or disable position pinning of GPS receiver" },
+ { 0x3A ,NULL,"Input Query position pinning Query position pinning status of the GPS receiver" },
+ { 0x3B ,NULL,"Input Configure position pinning parameters Set position pinning parameters of GPS receiver" },
+ { 0x3C ,NULL,"Input Configuration navigation mode Configure the navigation mode of GPS system" },
+ { 0x3D ,NULL,"Input Query navigation mode Query the navigation mode of GPS receiver" },
+ { 0x3E ,NULL,"Input Configure 1PPS mode Set 1PPS mode to the GPS receiver" },
+ { 0x3F ,NULL,"Input Query 1PPS mode Query 1PPS mode of the GPS receiver" },
+ { 0x80 ,Skytraq_0x80,"Output Software version Software revision of the receiver" },
+ { 0x81 ,NULL,"Output Software CRC Software CRC of the receiver" },
+ { 0x82 ,NULL,"Output Reserved Reserved" },
+ { 0x83 ,NULL,"Output ACK ACK to a successful input message" },
+ { 0x84 ,NULL,"Output NACK Response to an unsuccessful input message" },
+ { 0x86 ,NULL,"Output Position update rate Position update rate of GPS system" },
+ { 0x87 ,NULL,"Output GPS Almanac Data Outputting the GPS Almanac Data of GPS receiver"},
+ { 0xAE ,NULL,"Output GPS Datum Datum used by the GPS receiver"},
+ { 0xB1 ,NULL,"Output GPS Ephemeris Data Outputting the GPS Ephemeris Data of GPS receiver"},
+ { 0xB3 ,NULL,"Output GPS WAAS status WAAS status of the GPS receiver"},
+ { 0xB4 ,NULL,"Output GPS Position pinning status Position pinning status of the GPS receiver"},
+ { 0xB5 ,NULL,"Output GPS navigation mode Navigation mode of the GPS receiver"},
+ { 0xB6 ,NULL,"Output GPS 1PPS mode 1PPS mode of GPS receiver"},
+ { 0xDC ,NULL,"Output MEAS_TIME Measurement time information"},
+ { 0xDD ,NULL,"Output RAW_MEAS Raw channel measurements"},
+ { 0xDE ,NULL,"Output SV_CH_STATUS SV and Channel status information"},
+ { 0xDF ,NULL,"Output RCV_STATE GPS receiver navigation state"},
+ { 0xE0 ,NULL,"Output SUBFRAME Subframe buffer data"},
+ { 0xFF ,NULL,"****UNKNOWN****"}
+};
+
+char Skytraq_QUERY_SOFTWARE_VERSION[] = {0xA0,0xA1,0x00,0x02,0x02,0x00,0x02,0x0D,0x0A};
+
+unsigned char Skytraq_set_rate[] = {
+ 0x12, /* ID */
+ 0x00, /* 00: 1Hz / 01: 2Hz / 02: 4Hz / 03: 5Hz / 04: 10Hz / 05: 20Hz / Others: 20Hz */
+ 0x01, /* Meas_time Enabling */
+ 0x01, /* Raw_meas Enabling */
+ 0x00, /* SV_CH_Staus Enabling */
+ 0x00, /* RCV_State Enabling */
+ 0x00, /* Subframe Enabling */
+ 0x00 /* Attributes */
+};
+
+void Skytraq_bin_send(unsigned short LEN,unsigned char *body)
+{
+ unsigned short i;
+ unsigned char sum;
+ gps.putc(0xA0);
+ gps.putc(0xA1);
+ gps.putc((LEN >> 8) & 0x00ff);
+ gps.putc(LEN & 0x00ff);
+ sum = 0;
+ for(i = 0;i < LEN;i++){
+ gps.putc((char)(*(body+i)));
+ sum = sum ^ *(body+i);
+ }
+ gps.putc(sum);
+ gps.putc(0x0D);
+ gps.putc(0x0A);
+}
+
+void Skytraq_bin_parse(unsigned char c,Skytraq_bin_info_t *info)
+{
+ if(info->cjobst == 0){
+ if(c == 0xa0){
+ info->cjobst = 1;
+ } else {
+ info->cjobst = 0;
+ }
+ } else if(info->cjobst == 1){
+ if(c == 0xa1){
+ info->cjobst = 2;
+ } else {
+ info->cjobst = 0;
+ }
+ } else if(info->cjobst == 2){
+ info->len = c;
+ info->cjobst = 3;
+ } else if(info->cjobst == 3){
+ info->len = (info->len << 8) | c;
+ info->cjobst = 4;
+ info->sum = 0;
+ info->recv = 0;
+ if(info->len >= sizeof(info->body)){
+ info->cjobst = 0;
+ }
+ } else if(info->cjobst == 4){
+ if(info->recv == 0){
+ info->ID = c;
+ } else {
+ info->body[info->recv - 1] = c;
+ }
+ info->sum = info->sum ^ c;
+ info->recv++;
+ if(info->recv >= info->len){
+ info->cjobst = 5;
+ }
+ } else if(info->cjobst == 5){
+ //debug.printf("sum[%02x][%02x]\n",info->sum,c);
+ if(info->sum == c){
+ info->cjobst = 6;
+ } else {
+ info->cjobst = 0;
+ debug.printf("\nSUMERROR[%02x][%02x]\n",info->sum,c);
+ }
+ }
+}
+
+unsigned char wbuf[512];
+int wcnt = 0;
+
+int main() {
+ Skytraq_bin_info_t bin_info;
+ int i;
+ unsigned long recv_count = 0;
+ unsigned long recv_bytes = 0;
+ FILE *fp = NULL;
+ int save_done = 0;
+ unsigned char c;
+ int do_log = 0;
+
+ bin_info.cjobst = 0;
+
+ debug.format(8,Serial::None,1);
+ debug.baud(115200);
+ debug.printf("GPS Logger \"__S1315F__1\" Start (BUILD:[" __DATE__ "/" __TIME__ "])\x0d\x0a");
+
+ PPS.rise(pps_rise);
+
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("S1315F Logger");
+
+ lcd.locate(0,1);
+ lcd.printf("Please wait");
+ wait(1.0);
+
+ gps.format(8,Serial::None,1);
+ gps.baud(115200);
+ gps.recvStart();
+
+ Skytraq_set_rate[1] = 0x05;
+#ifdef OUTPUT_RATE_1HZ
+ Skytraq_set_rate[1] = 0x00;
+#endif
+ Skytraq_bin_send(sizeof(Skytraq_set_rate),Skytraq_set_rate);
+
+ timer.attach_us(timer_handler,1000*100);
+
+ while(1) {
+// for(i = 0;i < sizeof(Skytraq_QUERY_SOFTWARE_VERSION);i++){
+// gps.putc(Skytraq_QUERY_SOFTWARE_VERSION[i]);
+// }
+ if((logSW_state[0] == 0) && (logSW_state[0] == 0)){
+ LOGLED=0;
+ do_log = 0;
+#if 0
+ if(recv_bytes >= (2024*1024)){
+ fclose(fp);
+ fp = NULL;
+ debug.printf("FILE Close\n");
+ save_done = 1;
+ }
+#endif
+ }
+ if((logSW_state[0] != 0) && (logSW_state[0] != 0)){
+ LOGLED=0.20;
+ do_log = 1;
+#if 0
+ if(fp == NULL){
+ fp = fopen("/sd/rawdata.dat","w+b");
+ debug.printf("FILE Open=%p\n",fp);
+ wcnt = 0;
+ }
+#endif
+ }
+
+ if (gps.readable()) {
+ c = (unsigned char)(gps.getc());
+ Skytraq_bin_parse(c,&bin_info);
+ recv_bytes++;
+ if(fp != NULL){
+ wbuf[wcnt] = c;
+ wcnt++;
+ if(wcnt >= sizeof(wbuf)){
+ fwrite(wbuf,sizeof(wbuf[0]),sizeof(wbuf)/sizeof(wbuf[0]),fp);
+ wcnt = 0;
+ }
+ }
+ if(bin_info.cjobst == 6){
+ for(i = 0;Skytraq_ID_List[i].ID != 0xFF;i++){
+ if(bin_info.ID == Skytraq_ID_List[i].ID){
+ break;
+ }
+ }
+ recv_count++;
+ lcd.locate(0,1);
+ lcd.printf("R:%-10lu %c",recv_bytes,(do_log) ? '*' : ' ');
+ debug.printf("ID=%02x len=%5d [%s]\n",bin_info.ID,bin_info.len,Skytraq_ID_List[i].name);
+ //if(Skytraq_ID_List[i].func != NULL){
+ // (*Skytraq_ID_List[i].func)(&bin_info);
+ //}
+ bin_info.cjobst = 0;
+ }
+ //debug.printf("[%02x]",gps.getc());
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Dec 18 11:17:21 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e2ac27c8e93e