Index: trunk/firmware/sources/saigd/nmealib++/config.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/config.h b/trunk/firmware/sources/saigd/nmealib++/config.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/config.h (revision 116) +++ /dev/null (revision 117) @@ -1,34 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: config.h 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#ifndef __NMEA_CONFIG_H__ -#define __NMEA_CONFIG_H__ - -#define NMEA_VERSION ("0.5.3") -#define NMEA_VERSION_MAJOR (0) -#define NMEA_VERSION_MINOR (5) -#define NMEA_VERSION_PATCH (3) - -#define NMEA_CONVSTR_BUF (256) -#define NMEA_TIMEPARSE_BUF (256) - -#define NMEA_UNI - -#define NMEA_POSIX(x) x -#define NMEA_INLINE inline - -#if !defined(NDEBUG) && !defined(NMEA_CE) -# include -# define NMEA_ASSERT(x) assert(x) -#else -# define NMEA_ASSERT(x) -#endif - -#endif /* __NMEA_CONFIG_H__ */ Index: trunk/firmware/sources/saigd/nmealib++/context.c =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/context.c b/trunk/firmware/sources/saigd/nmealib++/context.c deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/context.c (revision 116) +++ /dev/null (revision 117) @@ -1,67 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: context.c 17 2008-03-11 11:56:11Z xtimor $ - * - */ - -#include "context.h" - -#include -#include -#include - -nmeaPROPERTY * nmea_property() -{ - static nmeaPROPERTY prop = { - 0, 0, NMEA_DEF_PARSEBUFF - }; - - return ∝ -} - -void nmea_trace(const char *str, ...) -{ - int size; - va_list arg_list; - char buff[NMEA_DEF_PARSEBUFF]; - nmeaTraceFunc func = nmea_property()->trace_func; - - if(func) - { - va_start(arg_list, str); - size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); - va_end(arg_list); - - if(size > 0) - (*func)(&buff[0], size); - } -} - -void nmea_trace_buff(const char *buff, int buff_size) -{ - nmeaTraceFunc func = nmea_property()->trace_func; - if(func && buff_size) - (*func)(buff, buff_size); -} - -void nmea_error(const char *str, ...) -{ - int size; - va_list arg_list; - char buff[NMEA_DEF_PARSEBUFF]; - nmeaErrorFunc func = nmea_property()->error_func; - - if(func) - { - va_start(arg_list, str); - size = NMEA_POSIX(vsnprintf)(&buff[0], NMEA_DEF_PARSEBUFF - 1, str, arg_list); - va_end(arg_list); - - if(size > 0) - (*func)(&buff[0], size); - } -} Index: trunk/firmware/sources/saigd/nmealib++/context.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/context.h b/trunk/firmware/sources/saigd/nmealib++/context.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/context.h (revision 116) +++ /dev/null (revision 117) @@ -1,44 +0,0 @@ -/* - * - * NMEA library - * URL: http://nmea.sourceforge.net - * Author: Tim (xtimor@gmail.com) - * Licence: http://www.gnu.org/licenses/lgpl.html - * $Id: context.h 4 2007-08-27 13:11:03Z xtimor $ - * - */ - -#ifndef __NMEA_CONTEXT_H__ -#define __NMEA_CONTEXT_H__ - -#include "config.h" - -#define NMEA_DEF_PARSEBUFF (1024) -#define NMEA_MIN_PARSEBUFF (256) - -#ifdef __cplusplus -extern "C" { -#endif - -typedef void (*nmeaTraceFunc)(const char *str, int str_size); -typedef void (*nmeaErrorFunc)(const char *str, int str_size); - -typedef struct _nmeaPROPERTY -{ - nmeaTraceFunc trace_func; - nmeaErrorFunc error_func; - int parse_buff_size; - -} nmeaPROPERTY; - -nmeaPROPERTY * nmea_property(); - -void nmea_trace(const char *str, ...); -void nmea_trace_buff(const char *buff, int buff_size); -void nmea_error(const char *str, ...); - -#ifdef __cplusplus -} -#endif - -#endif /* __NMEA_CONTEXT_H__ */ Index: trunk/firmware/sources/saigd/nmealib++/nmeaCode.cpp =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaCode.cpp b/trunk/firmware/sources/saigd/nmealib++/nmeaCode.cpp deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaCode.cpp (revision 116) +++ /dev/null (revision 117) @@ -1,48 +0,0 @@ -/* - * nemeaCode.cpp - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#include "nmeaCode.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -nmeaCode::nmeaCode(char* name=nullptr): -codebuff(nullptr){ - -} - -nmeaCode::~nmeaCode() { - // TODO Auto-generated destructor stub -} - - -bool nmeaCode::nmea_code_set(const char* cd) { - size_t sz = strlen(cd); - codebuff = new char[sz]; - strcpy(codebuff, cd); - return true; -} - -bool nmeaCode::nmea_code_get(char* cd) { - return true; -} - -//////////////////////////////////////////////////////////////////////////////////////// - - - - - - - Index: trunk/firmware/sources/saigd/nmealib++/nmeaCode.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaCode.h b/trunk/firmware/sources/saigd/nmealib++/nmeaCode.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaCode.h (revision 116) +++ /dev/null (revision 117) @@ -1,29 +0,0 @@ -/* - * nemeaCode.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB_NMEALIB___NMEACODE_H_ -#define NMEALIB_NMEALIB___NMEACODE_H_ - -#include "nmea_defs.h" - -class nmeaCode { -public: - nmeaCode(char* name=nullptr); - virtual ~nmeaCode(); - - bool nmea_code_set(const char* cd); - bool nmea_code_get(char* cd); - -private: - size_t size; - const char codename[8]; - const int type; - char *codebuff; - -}; - -#endif /* NMEALIB_NMEALIB___NMEACODE_H_ */ Index: trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.cpp =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.cpp b/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.cpp deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.cpp (revision 116) +++ /dev/null (revision 117) @@ -1,194 +0,0 @@ -/* - * nmeaGenerator.cpp - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#include -#include -#include "nmeaGenerator.h" - - -nmeaGenerator::nmeaGenerator() { - // TODO Auto-generated constructor stub - -} - -nmeaGenerator::~nmeaGenerator() { - // TODO Auto-generated destructor stub -} - - -virtual int nmeaGenerator::nmea_gen_info2code(const nmeaInfo *info, nmeaCode* code) -{ - return 1; -} - - -/* - * low level - */ - -int nmeaGenerator::nmea_gen_init(nmeaInfo *info) -{ - int RetVal = 1; int smask = info->smask; - - info->nmea_info_init_INFO(); - info->smask = smask; - - info->lat = NMEA_DEF_LAT; - info->lon = NMEA_DEF_LON; - - if(init_call) - RetVal = (*init_call)(this, info); - - if(RetVal && next) - RetVal = next->nmea_gen_init(info); - - return RetVal; -} - -int nmeaGenerator::nmea_gen_loop( nmeaInfo *info) -{ - int RetVal = 1; - - if(loop_call) - RetVal = (*loop_call)(this, info); - - if(RetVal && next) - RetVal = next->nmea_gen_loop(info); - - return RetVal; -} - -int nmeaGenerator::nmea_gen_reset(nmeaInfo *info) -{ - int RetVal = 1; - - if(reset_call) - RetVal = (*reset_call)(this, info); - - return RetVal; -} - -void nmeaGenerator::nmea_gen_destroy() -{ - if(next) - { - next->nmea_gen_destroy(); - next = 0; - } - - if(destroy_call) - (*destroy_call)(this); - - free(this); -} - -void nmeaGenerator::nmea_gen_add(nmeaGenerator *gen) -{ - if(next) - next->nmea_gen_add(gen); - else - next = gen; -} - -int nmeaGenerator::nmea_generate_from( - char *buff, int buff_sz, - nmeaInfo *info, - nmeaCode *code - ) -{ - int retval; - - if(0 != (retval = nmea_gen_loop(info))) - retval = nmea_generate(buff, buff_sz, info, code); - - return retval; -} - - - -int nmeaGenerator::nmea_igen_pos_rmove_destroy(void) -{ - return 1; -}; - -/* - * generator create - */ - -nmeaGenerator* nmeaGenerator::__nmea_create_generator(int type, nmeaInfo *info) -{ - nmeaGenerator* gen = nullptr; - switch(type) - { - case NMEA_GEN_NOISE: - gen = new nmeaGenerator(); - gen->init_call = &info->nmea_igen_noise_init; - gen->loop_call = &info->nmea_igen_noise_loop; - gen->reset_call = &info->nmea_igen_noise_reset; - break; - case NMEA_GEN_STATIC: - case NMEA_GEN_SAT_STATIC: - gen = new nmeaGenerator(); - gen->init_call = &info->nmea_igen_static_init; - gen->loop_call = &info->nmea_igen_static_loop; - gen->reset_call = &info->nmea_igen_static_reset; - break; - case NMEA_GEN_SAT_ROTATE: - gen = new nmeaGenerator(); - gen->init_call = &info->nmea_igen_rotate_init; - gen->loop_call = &info->nmea_igen_rotate_loop; - gen->reset_call = &info->nmea_igen_rotate_reset; - break; - case NMEA_GEN_POS_RANDMOVE: - gen = new nmeaGenerator(); - gen->init_call = &info->nmea_igen_pos_rmove_init; - gen->loop_call = &info->nmea_igen_pos_rmove_loop; - gen->destroy_call = &nmea_igen_pos_rmove_destroy; - break; - case NMEA_GEN_ROTATE: - gen = new nmeaGenerator(); - gen = __nmea_create_generator(NMEA_GEN_SAT_ROTATE, info); - gen->nmea_gen_add(__nmea_create_generator(NMEA_GEN_POS_RANDMOVE, info)); - break; - }; - return this; -} - -nmeaGenerator * nmeaGenerator::nmea_create_generator(int type, nmeaInfo *info) -{ - nmeaGenerator *gen = __nmea_create_generator(type, info); - - if(gen) - gen->nmea_gen_init(info); - - return gen; -} - -void nmeaGenerator::nmea_destroy_generator(void) -{ - nmea_gen_destroy(); -} - - - -int nmeaGenerator::nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode *code) -{ - int gen_count = 0; - - if(!buff) - return 0; - - while(code) - { - nmea_gen_info2code(info, code) - gen_count += code->code_gen(buff + gen_count, buff_sz - gen_count); - code += 1; - if(buff_sz - gen_count <= 0) - break; - } - return gen_count; -} Index: trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.h b/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaGenerator.h (revision 116) +++ /dev/null (revision 117) @@ -1,70 +0,0 @@ -/* - * nmeaGenerator.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB_NMEALIB___NMEAGENERATOR_H_ -#define NMEALIB_NMEALIB___NMEAGENERATOR_H_ - -#include "nmea_defs.h" -#include "nmeaCode.h" -#include "nmeaInfo.h" - -/* - * low level - */ - -typedef int (*nmeaNMEA_GEN_INIT)(nmeaGenerator *gen, nmeaInfo *info); -typedef int (*nmeaNMEA_GEN_LOOP)(nmeaGenerator *gen, nmeaInfo *info); -typedef int (*nmeaNMEA_GEN_RESET)(nmeaGenerator *gen, nmeaInfo *info); -typedef int (*nmeaNMEA_GEN_DESTROY)(nmeaGenerator *gen); - -enum nmeaGENTYPE -{ - NMEA_GEN_NOISE = 0, - NMEA_GEN_STATIC, - NMEA_GEN_ROTATE, - - NMEA_GEN_SAT_STATIC, - NMEA_GEN_SAT_ROTATE, - NMEA_GEN_POS_RANDMOVE, - - NMEA_GEN_LAST -}; - -class nmeaGenerator { -public: - nmeaGenerator(); - virtual ~nmeaGenerator(); - - - virtual int nmea_gen_info2code(const nmeaInfo *info, nmeaCode* code); - - int nmea_gen_init(nmeaInfo *info); - int nmea_gen_loop(nmeaInfo *info); - int nmea_gen_reset(nmeaInfo *info); - void nmea_gen_destroy(void); - void nmea_gen_add(nmeaGenerator *from); - int nmea_generate(char *buff, int buff_sz, const nmeaInfo *info, nmeaCode* code); - -private: - void *gen_data; - nmeaNMEA_GEN_INIT init_call; - nmeaNMEA_GEN_LOOP loop_call; - nmeaNMEA_GEN_RESET reset_call; - nmeaNMEA_GEN_DESTROY destroy_call; - nmeaGenerator *next; - - int nmea_generate_from(char *buff, int buff_sz,nmeaInfo *info, nmeaCode *code); - int nmea_igen_pos_rmove_destroy(void); - - - nmeaGenerator * __nmea_create_generator(int type, nmeaInfo *info); - nmeaGenerator * nmea_create_generator(int type, nmeaInfo *info); - void nmea_destroy_generator(void); - -}; - -#endif /* NMEALIB_NMEALIB___NMEAGENERATOR_H_ */ Index: trunk/firmware/sources/saigd/nmealib++/nmeaInfo.cpp =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.cpp b/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.cpp deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.cpp (revision 116) +++ /dev/null (revision 117) @@ -1,257 +0,0 @@ -/* - * nmeaInfo.cpp - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#include "utils.h" -#include "nmeaInfo.h" - -nmeaInfo::nmeaInfo() { - // TODO Auto-generated constructor stub - -} - -nmeaInfo::~nmeaInfo() { - // TODO Auto-generated destructor stub -} - -void nmeaInfo::nmea_info_init_INFO() -{ - - nmea_info_time_now(); - sig = NMEA_SIG_BAD; - fix = NMEA_FIX_BAD; -} - -static void nmeaInfo::nmea_info_time_now() { - time_t lt; - struct tm *tt; - - time(<); - tt = gmtime(<); - - utc.year = tt->tm_year; - utc.mon = tt->tm_mon; - utc.day = tt->tm_mday; - utc.hour = tt->tm_hour; - utc.min = tt->tm_min; - utc.sec = tt->tm_sec; - utc.hsec = 0; -} - -/** - * \brief Convert position from INFO to radians position - */ -void nmeaInfo::nmea_info_info2pos(nmeaPOS *pos) { - pos->lat = code_ndeg2radian(lat); - pos->lon = code_ndeg2radian(lon); -} - -/** - * \brief Convert radians position to INFOs position - */ -void nmeaInfo::nmea_info_pos2info(const nmeaPOS *pos) { - lat = code_radian2ndeg(pos->lat); - lon = code_radian2ndeg(pos->lon); -} - - - -/* - * NOISE generator - */ - -int nmeaInfo::nmea_igen_noise_init() -{ - return 1; -} - -int nmeaInfo::nmea_igen_noise_loop() -{ - int it; - int in_use; - - sig = (int)nmea_random(1, 3); - PDOP = nmea_random(0, 9); - HDOP = nmea_random(0, 9); - VDOP = nmea_random(0, 9); - fix = (int)nmea_random(2, 3); - lat = nmea_random(0, 100); - lon = nmea_random(0, 100); - speed = nmea_random(0, 100); - direction = nmea_random(0, 360); - declination = nmea_random(0, 360); - elv = (int)nmea_random(-100, 100); - - satinfo.inuse = 0; - satinfo.inview = 0; - - for(it = 0; it < 12; ++it) - { - satinfo.sat[it].id = it; - satinfo.sat[it].in_use = in_use = (int)nmea_random(0, 3); - satinfo.sat[it].elv = (int)nmea_random(0, 90); - satinfo.sat[it].azimuth = (int)nmea_random(0, 359); - satinfo.sat[it].sig = (int)(in_use?nmea_random(40, 99):nmea_random(0, 40)); - - if(in_use) - satinfo.inuse++; - if(satinfo.sat[it].sig > 0) - satinfo.inview++; - } - - return 1; -} - -int nmeaInfo::nmea_igen_noise_reset() -{ - return 1; -} - -/* - * STATIC generator - */ - -int nmeaInfo::nmea_igen_static_loop() -{ - nmea_info_time_now(); - return 1; -}; - -int nmeaInfo::nmea_igen_static_reset() -{ - satinfo.inuse = 4; - satinfo.inview = 4; - - satinfo.sat[0].id = 1; - satinfo.sat[0].in_use = 1; - satinfo.sat[0].elv = 50; - satinfo.sat[0].azimuth = 0; - satinfo.sat[0].sig = 99; - - satinfo.sat[1].id = 2; - satinfo.sat[1].in_use = 1; - satinfo.sat[1].elv = 50; - satinfo.sat[1].azimuth = 90; - satinfo.sat[1].sig = 99; - - satinfo.sat[2].id = 3; - satinfo.sat[2].in_use = 1; - satinfo.sat[2].elv = 50; - satinfo.sat[2].azimuth = 180; - satinfo.sat[2].sig = 99; - - satinfo.sat[3].id = 4; - satinfo.sat[3].in_use = 1; - satinfo.sat[3].elv = 50; - satinfo.sat[3].azimuth = 270; - satinfo.sat[3].sig = 99; - - return 1; -} - -int nmeaInfo::nmea_igen_static_init() -{ - sig = 3; - fix = 3; - - nmea_igen_static_reset(); - - return 1; -} - -/* - * SAT_ROTATE generator - */ - -int nmeaInfo::nmea_igen_rotate_loop() -{ - int it; - int count = satinfo.inview; - double deg = 360 / (count?count:1); - double srt = (count?(satinfo.sat[0].azimuth):0) + 5; - - nmea_info_time_now(); - - for(it = 0; it < count; ++it) - { - satinfo.sat[it].azimuth = - (int)((srt >= 360)?srt - 360:srt); - srt += deg; - } - - return 1; -}; - -int nmeaInfo::nmea_igen_rotate_reset() -{ - int it; - double deg = 360 / 8; - double srt = 0; - - satinfo.inuse = 8; - satinfo.inview = 8; - - for(it = 0; it < satinfo.inview; ++it) - { - satinfo.sat[it].id = it + 1; - satinfo.sat[it].in_use = 1; - satinfo.sat[it].elv = 5; - satinfo.sat[it].azimuth = (int)srt; - satinfo.sat[it].sig = 80; - srt += deg; - } - - return 1; -} - -int nmeaInfo::nmea_igen_rotate_init() -{ - sig = 3; - fix = 3; - - nmea_igen_rotate_reset(); - - return 1; -} - -/* - * POS_RANDMOVE generator - */ - -int nmeaInfo::nmea_igen_pos_rmove_init() -{ - sig = 3; - fix = 3; - direction = declination = 0; - speed = 20; - return 1; -} - -int nmeaInfo::nmea_igen_pos_rmove_loop() -{ - nmeaPOS crd; - - direction += nmea_random(-10, 10); - speed += nmea_random(-2, 3); - - if(direction < 0) - direction = 359 + direction; - if(direction > 359) - direction -= 359; - - if(speed > 40) - speed = 40; - if(speed < 1) - speed = 1; - - nmea_info_info2pos(&crd); - code_move_horz(&crd, &crd, direction, speed / 3600); - nmea_info_pos2info(&crd); - - declination = direction; - - return 1; -}; Index: trunk/firmware/sources/saigd/nmealib++/nmeaInfo.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.h b/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaInfo.h (revision 116) +++ /dev/null (revision 117) @@ -1,92 +0,0 @@ -/* - * nmeaInfo.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB___NMEAINFO_H_ -#define NMEALIB___NMEAINFO_H_ - -#include "nmea_defs.h" -/** - * Information about satellite - * @see nmeaSATINFO - * @see nmeaGPGSV - */ -typedef struct _nmeaSATELLITE -{ - int id; /**< Satellite PRN number */ - int in_use; /**< Used in position fix */ - int elv; /**< Elevation in degrees, 90 maximum */ - int azimuth; /**< Azimuth, degrees from true north, 000 to 359 */ - int sig; /**< Signal, 00-99 dB */ - -} nmeaSATELLITE; - -/** - * Information about all satellites in view - * @see nmeaINFO - * @see nmeaGPGSV - */ -typedef struct _nmeaSATINFO -{ - int inuse; /**< Number of satellites in use (not those in view) */ - int inview; /**< Total number of satellites in view */ - nmeaSATELLITE sat[NMEA_MAXSAT]; /**< Satellites information */ - -} nmeaSATINFO; - -/** - * Summary GPS information from all parsed packets, - * used also for generating NMEA stream - * @see nmea_parse - * @see nmea_GPGGA2info, nmea_...2info - */ -class nmeaInfo { -public: - nmeaInfo(); - virtual ~nmeaInfo(); - void nmea_info_init_INFO(); - static void nmea_info_time_now(); - void nmea_info_info2pos(nmeaPOS *pos); - void nmea_info_pos2info(const nmeaPOS *pos); - - int nmea_info_noise_init(); - int nmea_info_noise_loop(); - int nmea_info_noise_reset(); - int nmea_info_static_loop(); - int nmea_info_static_reset(); - int nmea_info_static_init(); - int nmea_info_rotate_loop(); - int nmea_info_rotate_reset(); - int nmea_info_rotate_init(); - int nmea_info_pos_rmove_init(); - int nmea_info_pos_rmove_loop(); - -private: - int smask; /**< Mask specifying types of packages from which data have been obtained */ - nmeaTIME utc; /**< UTC of position */ - int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */ - int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */ - double PDOP; /**< Position Dilution Of Precision */ - double HDOP; /**< Horizontal Dilution Of Precision */ - double VDOP; /**< Vertical Dilution Of Precision */ - double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */ - double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */ - double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */ - double speed; /**< Speed over the ground in kilometers/hour */ - double direction; /**< Track angle in degrees True */ - double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */ - char ns; - char ew; - double satinuse; - char elv_units; - double diff; - char diff_units; - double dgps_age; - double dgps_sid; - nmeaSATINFO satinfo; /**< Satellites information */ -}; - -#endif /* NMEALIB___NMEAINFO_H_ */ Index: trunk/firmware/sources/saigd/nmealib++/nmeaParser.cpp =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaParser.cpp b/trunk/firmware/sources/saigd/nmealib++/nmeaParser.cpp deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaParser.cpp (revision 116) +++ /dev/null (revision 117) @@ -1,309 +0,0 @@ -/* - * nmeaParser.cpp - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#include "nmea_defs.h" -#include "nmeaParser.h" - -/** - * \brief Initialization of parser object - */ -nmeaParser::nmeaParser(): -top_node(nullptr),end_node(nullptr), -buffer(nullptr),buff_size(0), -buff_use(0){ - int buff_sz = nmea_property()->parse_buff_size; - NMEA_ASSERT(this); - - if(buff_sz < NMEA_MIN_PARSEBUFF) - buff_sz = NMEA_MIN_PARSEBUFF; - - if(0 == (buffer = malloc(buff_size))) - nmea_error("Insufficient memory!"); - delete this; - else - { - buff_size = buff_sz; - - } -} - -/** - * \brief Destroy parser object - */ -nmeaParser::~nmeaParser() { - NMEA_ASSERT(this && buffer); - free(buffer); - nmea_parser_queue_clear(); -} - -/** - * \brief Analysis of buffer and put results to information structure - * @return Number of packets wos parsed - */ -int nmeaParser::nmea_parse(const char *buff, int buff_sz, nmeaInfo *info ) -{ - int ptype, nread = 0; - nmeaCode *pack = 0; - - NMEA_ASSERT(this && buffer); - - nmea_parser_push(buff, buff_sz); - - while(GPNON != (ptype = nmea_parser_pop(&pack))) - { - nread++; - code_2info(pack, info); - free(pack); - } - - return nread; -} - -/* - * low level - */ - -int nmeaParser::nmea_parser_real_push(const char *buff, int buff_sz) -{ - int nparsed = 0, crc, sen_sz, ptype; - nmeaParserNODE *node = 0; - - NMEA_ASSERT(this && buffer); - - /* add */ - if(buff_use + buff_sz >= buff_size) - nmea_parser_buff_clear(parser); - - memcpy(buffer + buff_use, buff, buff_sz); - buff_use += buff_sz; - - /* parse */ - for(;;node = 0) - { - sen_sz = nmea_find_tail( - (const char *)buffer + nparsed, - (int)buff_use - nparsed, &crc); - - if(!sen_sz) - { - if(nparsed) - memcpy( - buffer, - buffer + nparsed, - buff_use -= nparsed); - break; - } - else if(crc >= 0) - { - if(!node->pack->code_parse( (const char *)buffer + nparsed, - sen_sz) - { - free(node); - node = 0; - } - - if(node) - { - if(end_node) - ((nmeaParserNODE *)end_node)->next_node = node; - end_node = node; - if(!top_node) - top_node = node; - node->next_node = 0; - } - } - - nparsed += sen_sz; - } - return nparsed; -} - -/** - * \brief Analysis of buffer and keep results into parser - * @return Number of bytes wos parsed from buffer - */ -int nmeaParser::nmea_parser_push(const char *buff, int buff_sz) -{ - int nparse, nparsed = 0; - - do - { - if(buff_sz > buff_size) - nparse = buff_size; - else - nparse = buff_sz; - - nparsed += nmea_parser_real_push( - buff, nparse); - - buff_sz -= nparse; - - } while(buff_sz); - - return nparsed; -} - -/** - * \brief Get type of top packet keeped into parser - * @return Type of packet - * @see nmeaPACKTYPE - */ -int nmeaParser::nmea_parser_top() -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)top_node; - - NMEA_ASSERT(this && buffer); - - if(node) - retval = node->pack->type; - - return retval; -} - -/** - * \brief Withdraw top packet from parser - * @return Received packet type - * @see nmeaPACKTYPE - */ -int nmeaParser::nmea_parser_pop( nmeaCode **pack_ptr) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)top_node; - - NMEA_ASSERT(this && buffer); - - if(node) - { - *pack_ptr = node->pack; - retval = node->pack->type; - top_node = node->next_node; - if(!top_node) - end_node = 0; - free(node); - } - return retval; -} - -/** - * \brief Get top packet from parser without withdraw - * @return Received packet type - * @see nmeaPACKTYPE - */ -int nmeaParser::nmea_parser_peek(nmeaCode **pack_ptr) -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)top_node; - - NMEA_ASSERT(this && buffer); - - if(node) - { - *pack_ptr = node->pack; - retval = node->pack->type; - } - - return retval; -} - -/** - * \brief Delete top packet from parser - * @return Deleted packet type - * @see nmeaPACKTYPE - */ -int nmeaParser::nmea_parser_drop() -{ - int retval = GPNON; - nmeaParserNODE *node = (nmeaParserNODE *)top_node; - - NMEA_ASSERT(this && buffer); - - if(node) - { - if(node->pack) - free(node->pack); - retval = node->pack->type; - top_node = node->next_node; - if(!top_node) - end_node = 0; - free(node); - } - - return retval; -} - -/** - * \brief Clear cache of parser - * @return true (1) - success - */ -int nmeaParser::nmea_parser_buff_clear() -{ - NMEA_ASSERT(this && buffer); - buff_use = 0; - return 1; -} - -/** - * \brief Clear packets queue into parser - * @return true (1) - success - */ -int nmeaParser::nmea_parser_queue_clear() -{ - NMEA_ASSERT(this); - while(top_node) - nmea_parser_drop(); - return 1; -} - -virtual void nmeaParser::code_2info(nmeaCode* code, nmeaInfo* info) { - -} - - -int nmeaParser::nmea_find_tail(const char *buff, int buff_sz, int *res_crc) -{ - static const int tail_sz = 3 /* *[CRC] */ + 2 /* \r\n */; - - const char *end_buff = buff + buff_sz; - int nread = 0; - int crc = 0; - - NMEA_ASSERT(buff && res_crc); - - *res_crc = -1; - - for(;buff < end_buff; ++buff, ++nread) - { - if(('$' == *buff) && nread) - { - buff = 0; - break; - } - else if('*' == *buff) - { - if(buff + tail_sz <= end_buff && '\r' == buff[3] && '\n' == buff[4]) - { - *res_crc = code_atoi(buff + 1, 2, 16); - nread = buff_sz - (int)(end_buff - (buff + tail_sz)); - if(*res_crc != crc) - { - *res_crc = -1; - buff = 0; - } - } - - break; - } - else if(nread) - crc ^= (int)*buff; - } - - if(*res_crc < 0 && buff) - nread = 0; - - return nread; -} Index: trunk/firmware/sources/saigd/nmealib++/nmeaParser.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmeaParser.h b/trunk/firmware/sources/saigd/nmealib++/nmeaParser.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmeaParser.h (revision 116) +++ /dev/null (revision 117) @@ -1,120 +0,0 @@ -/* - * nmeaParser.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB_NMEALIB___NMEAPARSER_H_ -#define NMEALIB_NMEALIB___NMEAPARSER_H_ - -typedef struct _nmeaParserNODE -{ - nmeaCode* pack; - struct _nmeaParserNODE *next_node; - -} nmeaParserNODE; - -class nmeaParser { -public: - nmeaParser(); - virtual ~nmeaParser(); - - int nmea_find_tail(const char *buff, int buff_sz, int *res_crc) - int nmea_parse( const char *buff, int buff_sz, nmeaInfo *info ); - - int nmea_parser_push(const char *buff, int buff_sz); - int nmea_parser_top(void); - int nmea_parser_pop(nmeaCode **pack_ptr); - int nmea_parser_peek(nmeaCode **pack_ptr); - int nmea_parser_drop(void); - int nmea_parser_buff_clear(void); - int nmea_parser_queue_clear(void); - - virtual void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); - -private: - void *top_node; - void *end_node; - unsigned char *buffer; - int buff_size; - int buff_use; - - int nmea_parser_real_push(const char *buff, int buff_sz); -}; - - -class nmeaGPALMParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -//codigos NMEA que se pueden enviar al receptor GPS GARMIN -class nmeaPGRMIParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMCParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMC1Parser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMOParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - - -//Codigos NMEA que puede transmitir el receptor GNSS - -class nmeaGPGGAParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaGPGSAParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaGPGSVParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaGPRMCParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaGPVTGParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaGPGLLParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -//codigos de propiedad de GARMIN -class nmeaPGRMEParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMFParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMMParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMTParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMVParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -class nmeaPGRMBParser:nmeaParser { - void nmea_parser_code2info(nmeaCode* code, nmeaInfo* info); -}; - -#endif /* NMEALIB_NMEALIB___NMEAPARSER_H_ */ Index: trunk/firmware/sources/saigd/nmealib++/nmea_defs.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/nmea_defs.h b/trunk/firmware/sources/saigd/nmealib++/nmea_defs.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/nmea_defs.h (revision 116) +++ /dev/null (revision 117) @@ -1,107 +0,0 @@ -/* - * nmea_defs.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB_NMEALIB___NMEA_DEFS_H_ -#define NMEALIB_NMEALIB___NMEA_DEFS_H_ - -#include "context.h" -/** - * - */ -#define code_PI (3.141592653589793) /**< PI value */ -#define code_PI180 (code_PI / 180) /**< PI division by 180 */ -#define code_EARTHRADIUS_KM (6378) /**< Earth's mean radius in km */ -#define code_EARTHRADIUS_M (code_EARTHRADIUS_KM * 1000) /**< Earth's mean radius in m */ -#define code_EARTH_SEMIMAJORAXIS_M (6378137.0) /**< Earth's semi-major axis in m according WGS84 */ -#define code_EARTH_SEMIMAJORAXIS_KM (code_EARTHMAJORAXIS_KM / 1000) /**< Earth's semi-major axis in km according WGS 84 */ -#define code_EARTH_FLATTENING (1 / 298.257223563) /**< Earth's flattening according WGS 84 */ -#define code_DOP_FACTOR (5) /**< Factor for translating DOP to meters */ - - -/* - * Distance units - */ -#define NMEA_TUD_YARDS (1.0936) /**< Yeards, meter * NMEA_TUD_YARDS = yard */ -#define NMEA_TUD_KNOTS (1.852) /**< Knots, kilometer / NMEA_TUD_KNOTS = knot */ -#define NMEA_TUD_MILES (1.609) /**< Miles, kilometer / NMEA_TUD_MILES = mile */ - -/* - * Speed units - */ -#define NMEA_TUS_MS (3.6) /**< Meters per seconds, (k/h) / NMEA_TUS_MS= (m/s) */ - - -#define NMEA_SIG_BAD (0) -#define NMEA_SIG_LOW (1) -#define NMEA_SIG_MID (2) -#define NMEA_SIG_HIGH (3) - -#define NMEA_FIX_BAD (1) -#define NMEA_FIX_2D (2) -#define NMEA_FIX_3D (3) - -#define NMEA_MAXSAT (12) -#define NMEA_SATINPACK (4) -#define NMEA_NSATPACKS (NMEA_MAXSAT / NMEA_SATINPACK) - -#define NMEA_DEF_LAT (5001.2621) -#define NMEA_DEF_LON (3613.0595) - -#define NMEA_TOKS_COMPARE (1) -#define NMEA_TOKS_PERCENT (2) -#define NMEA_TOKS_WIDTH (3) -#define NMEA_TOKS_TYPE (4) - - -/** - * NMEA packets type which parsed and generated by library - */ -enum nmeaPACKTYPE -{ - GPNON = 0x0000, /**< Unknown packet type. */ - GPGGA = 0x0001, /**< GGA - Essential fix data which provide 3D location and accuracy data. */ - GPGSA = 0x0002, /**< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values. */ - GPGSV = 0x0004, /**< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values. */ - GPRMC = 0x0008, /**< RMC - Recommended Minimum Specific GPS/TRANSIT Data. */ - GPVTG = 0x0010, /**< VTG - Actual track made good and speed over ground. */ - GPALM = 0x0020, - GPGLL = 0x0040, - PGRMI = 0x0080, - PGRMC = 0x0100, - PGRMC1 = 0x0200, - PGRMO = 0x0400 -}; - - -/** - * Position data in fractional degrees or radians - */ -typedef struct _nmeaPOS -{ - double lat; /**< Latitude */ - double lon; /**< Longitude */ - -} nmeaPOS; - - -/** - * Date and time data - * @see nmea_time_now - */ -typedef struct _nmeaTIME -{ - int year; /**< Years since 1900 */ - int mon; /**< Months since January - [0,11] */ - int day; /**< Day of the month - [1,31] */ - int hour; /**< Hours since midnight - [0,23] */ - int min; /**< Minutes after the hour - [0,59] */ - int sec; /**< Seconds after the minute - [0,59] */ - int hsec; /**< Hundredth part of second - [0,99] */ - -} nmeaTIME; - -#endif /* NMEALIB_NMEALIB___NMEA_DEFS_H_ */ Index: trunk/firmware/sources/saigd/nmealib++/utils.cpp =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/utils.cpp b/trunk/firmware/sources/saigd/nmealib++/utils.cpp deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/utils.cpp (revision 116) +++ /dev/null (revision 117) @@ -1,590 +0,0 @@ -/* - * utils.cpp - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#include -#include -#include "utils.h" - -/** - * \fn code_degree2radian - * \brief Convert degree to radian - */ -double nmea_utils_degree2radian(double val) { - return (val * code_PI180); -} - -/** - * \fn code_radian2degree - * \brief Convert radian to degree - */ -double nmea_utils_radian2degree(double val) { - return (val / code_PI180); -} - -/** - * \brief Convert NDEG (NMEA degree) to fractional degree - */ -double nmea_utils_ndeg2degree(double val) { - double deg = ((int)(val / 100)); - val = deg + (val - deg * 100) / 60; - return val; -} - -/** - * \brief Convert fractional degree to NDEG (NMEA degree) - */ -double nmea_utils_degree2ndeg(double val) { - double int_part; - double fra_part; - fra_part = modf(val, &int_part); - val = int_part * 100 + fra_part * 60; - return val; -} - -/** - * \fn code_ndeg2radian - * \brief Convert NDEG (NMEA degree) to radian - */ -double code_ndeg2radian(double val) { - return nmea_utils_degree2radian(nmea_utils_ndeg2degree(val)); -} - -/** - * \fn code_radian2ndeg - * \brief Convert radian to NDEG (NMEA degree) - */ -double nmea_utils_radian2ndeg(double val) { - return nmea_utils_degree2ndeg(nmea_utils_radian2degree(val)); -} - -/** - * \brief Calculate PDOP (Position Dilution Of Precision) factor - */ -double nmea_utils_calc_pdop(double hdop, double vdop) { - return sqrt(pow(hdop, 2) + pow(vdop, 2)); -} - -double nmea_utils_dop2meters(double dop) { - return (dop * code_DOP_FACTOR); -} - -double nmea_utils_meters2dop(double meters) { - return (meters / code_DOP_FACTOR); -} - -/** - * \brief Calculate distance between two points - * \return Distance in meters - */ -double nmea_utils_distance( - const nmeaPOS *from_pos, /**< From position in radians */ - const nmeaPOS *to_pos /**< To position in radians */ - ) -{ - double dist = ((double)code_EARTHRADIUS_M) * acos( - sin(to_pos->lat) * sin(from_pos->lat) + - cos(to_pos->lat) * cos(from_pos->lat) * cos(to_pos->lon - from_pos->lon) - ); - return dist; -} - -/** - * \brief Calculate distance between two points - * This function uses an algorithm for an oblate spheroid earth model. - * The algorithm is described here: - * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf - * \return Distance in meters - */ -double nmea_utils_distance_ellipsoid( - const nmeaPOS *from_pos, /**< From position in radians */ - const nmeaPOS *to_pos, /**< To position in radians */ - double *from_azimuth, /**< (O) azimuth at "from" position in radians */ - double *to_azimuth /**< (O) azimuth at "to" position in radians */ - ) -{ - /* All variables */ - double f, a, b, sqr_a, sqr_b; - double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; - double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; - int remaining_steps; - double sqr_u, A, B, delta_sigma; - - /* Check input */ - NMEA_ASSERT(from_pos != 0); - NMEA_ASSERT(to_pos != 0); - - if ((from_pos->lat == to_pos->lat) && (from_pos->lon == to_pos->lon)) - { /* Identical points */ - if ( from_azimuth != 0 ) - *from_azimuth = 0; - if ( to_azimuth != 0 ) - *to_azimuth = 0; - return 0; - } /* Identical points */ - - /* Earth geometry */ - f = code_EARTH_FLATTENING; - a = code_EARTH_SEMIMAJORAXIS_M; - b = (1 - f) * a; - sqr_a = a * a; - sqr_b = b * b; - - /* Calculation */ - L = to_pos->lon - from_pos->lon; - phi1 = from_pos->lat; - phi2 = to_pos->lat; - U1 = atan((1 - f) * tan(phi1)); - U2 = atan((1 - f) * tan(phi2)); - sin_U1 = sin(U1); - sin_U2 = sin(U2); - cos_U1 = cos(U1); - cos_U2 = cos(U2); - - /* Initialize iteration */ - sigma = 0; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - cos_2_sigmam = 0; - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - sqr_cos_alpha = 0; - lambda = L; - sin_lambda = sin(lambda); - cos_lambda = cos(lambda); - delta_lambda = lambda; - remaining_steps = 20; - - while ((delta_lambda > 1e-12) && (remaining_steps > 0)) - { /* Iterate */ - /* Variables */ - double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; - - /* Calculation */ - tmp1 = cos_U2 * sin_lambda; - tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; - sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); - cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; - tan_sigma = sin_sigma / cos_sigma; - sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; - cos_alpha = cos(asin(sin_alpha)); - sqr_cos_alpha = cos_alpha * cos_alpha; - cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); - lambda_prev = lambda; - sigma = asin(sin_sigma); - lambda = L + - (1 - C) * f * sin_alpha - * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); - delta_lambda = lambda_prev - lambda; - if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; - sin_lambda = sin(lambda); - cos_lambda = cos(lambda); - remaining_steps--; - } /* Iterate */ - - /* More calculation */ - sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; - A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); - B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); - delta_sigma = B * sin_sigma * ( - cos_2_sigmam + B / 4 * ( - cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - - B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) - )); - - /* Calculate result */ - if ( from_azimuth != 0 ) - { - double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); - *from_azimuth = atan(tan_alpha_1); - } - if ( to_azimuth != 0 ) - { - double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); - *to_azimuth = atan(tan_alpha_2); - } - - return b * A * (sigma - delta_sigma); - -} -/** - * \brief Horizontal move of point position - */ -int nmea_utils_move_horz( - const nmeaPOS *start_pos, /**< Start position in radians */ - nmeaPOS *end_pos, /**< Result position in radians */ - double azimuth, /**< Azimuth (degree) [0, 359] */ - double distance /**< Distance (km) */ -) -{ - nmeaPOS p1 = *start_pos; - int RetVal = 1; - - distance /= code_EARTHRADIUS_KM; /* Angular distance covered on earth's surface */ - azimuth = nmea_utils_degree2radian(azimuth); - - end_pos->lat = asin( - sin(p1.lat) * cos(distance) + cos(p1.lat) * sin(distance) * cos(azimuth)); - end_pos->lon = p1.lon + atan2( - sin(azimuth) * sin(distance) * cos(p1.lat), cos(distance) - sin(p1.lat) * sin(end_pos->lat)); - - if(NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)) - { - end_pos->lat = 0; end_pos->lon = 0; - RetVal = 0; - } - - return RetVal; -} - -/** - * \brief Horizontal move of point position - * This function uses an algorithm for an oblate spheroid earth model. - * The algorithm is described here: - * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf - */ -int nmea_utils_move_horz_ellipsoid( - const nmeaPOS *start_pos, /**< Start position in radians */ - nmeaPOS *end_pos, /**< (O) Result position in radians */ - double azimuth, /**< Azimuth in radians */ - double distance, /**< Distance (km) */ - double *end_azimuth /**< (O) Azimuth at end position in radians */ -) -{ - /* Variables */ - double f, a, b, sqr_a, sqr_b; - double phi1, tan_U1, sin_U1, cos_U1, s, alpha1, sin_alpha1, cos_alpha1; - double tan_sigma1, sigma1, sin_alpha, cos_alpha, sqr_cos_alpha, sqr_u, A, B; - double sigma_initial, sigma, sigma_prev, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, delta_sigma; - int remaining_steps; - double tmp1, phi2, lambda, C, L; - - /* Check input */ - NMEA_ASSERT(start_pos != 0); - NMEA_ASSERT(end_pos != 0); - - if (fabs(distance) < 1e-12) - { /* No move */ - *end_pos = *start_pos; - if ( end_azimuth != 0 ) *end_azimuth = azimuth; - return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); - } /* No move */ - - /* Earth geometry */ - f = code_EARTH_FLATTENING; - a = code_EARTH_SEMIMAJORAXIS_M; - b = (1 - f) * a; - sqr_a = a * a; - sqr_b = b * b; - - /* Calculation */ - phi1 = start_pos->lat; - tan_U1 = (1 - f) * tan(phi1); - cos_U1 = 1 / sqrt(1 + tan_U1 * tan_U1); - sin_U1 = tan_U1 * cos_U1; - s = distance; - alpha1 = azimuth; - sin_alpha1 = sin(alpha1); - cos_alpha1 = cos(alpha1); - tan_sigma1 = tan_U1 / cos_alpha1; - sigma1 = atan2(tan_U1, cos_alpha1); - sin_alpha = cos_U1 * sin_alpha1; - sqr_cos_alpha = 1 - sin_alpha * sin_alpha; - cos_alpha = sqrt(sqr_cos_alpha); - sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; - A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); - B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); - - /* Initialize iteration */ - sigma_initial = s / (b * A); - sigma = sigma_initial; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - cos_2_sigmam = cos(2 * sigma1 + sigma); - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - delta_sigma = 0; - sigma_prev = 2 * code_PI; - remaining_steps = 20; - - while ((fabs(sigma - sigma_prev) > 1e-12) && (remaining_steps > 0)) - { /* Iterate */ - cos_2_sigmam = cos(2 * sigma1 + sigma); - sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; - sin_sigma = sin(sigma); - cos_sigma = cos(sigma); - delta_sigma = B * sin_sigma * ( - cos_2_sigmam + B / 4 * ( - cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - - B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) - )); - sigma_prev = sigma; - sigma = sigma_initial + delta_sigma; - remaining_steps --; - } /* Iterate */ - - /* Calculate result */ - tmp1 = (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1); - phi2 = atan2( - sin_U1 * cos_sigma + cos_U1 * sin_sigma * cos_alpha1, - (1 - f) * sqrt(sin_alpha * sin_alpha + tmp1 * tmp1) - ); - lambda = atan2( - sin_sigma * sin_alpha1, - cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1 - ); - C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); - L = lambda - - (1 - C) * f * sin_alpha * ( - sigma + C * sin_sigma * - (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)) - ); - - /* Result */ - end_pos->lon = start_pos->lon + L; - end_pos->lat = phi2; - if ( end_azimuth != 0 ) - { - *end_azimuth = atan2( - sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1 - ); - } - return ! (NMEA_POSIX(isnan)(end_pos->lat) || NMEA_POSIX(isnan)(end_pos->lon)); -} - -/** - * \brief Calculate control sum of binary buffer - */ -int nmea_utils_calc_crc(const char *buff, int buff_sz) -{ - int chsum = 0, - it; - - for(it = 0; it < buff_sz; ++it) - chsum ^= (int)buff[it]; - - return chsum; -} - -/** - * \brief Convert string to number - */ -int nmea_utils_atoi(const char *str, int str_sz, int radix) -{ - char *tmp_ptr; - char buff[NMEA_CONVSTR_BUF]; - int res = 0; - - if(str_sz < NMEA_CONVSTR_BUF) - { - memcpy(&buff[0], str, str_sz); - buff[str_sz] = '\0'; - res = strtol(&buff[0], &tmp_ptr, radix); - } - - return res; -} - -/** - * \brief Convert string to fraction number - */ -double nmea_utils_atof(const char *str, int str_sz) -{ - char *tmp_ptr; - char buff[NMEA_CONVSTR_BUF]; - double res = 0; - - if(str_sz < NMEA_CONVSTR_BUF) - { - memcpy(&buff[0], str, str_sz); - buff[str_sz] = '\0'; - res = strtod(&buff[0], &tmp_ptr); - } - - return res; -} - -/** - * \brief Formating string (like standart printf) with CRC tail (*CRC) - */ -int nmea_utils_printf(char *buff, int buff_sz, const char *format, ...) -{ - int retval, add = 0; - va_list arg_ptr; - - if(buff_sz <= 0) - return 0; - - va_start(arg_ptr, format); - - retval = NMEA_POSIX(vsnprintf)(buff, buff_sz, format, arg_ptr); - - if(retval > 0) - { - add = NMEA_POSIX(snprintf)( - buff + retval, buff_sz - retval, "*%02x\r\n", - nmea_utils_calc_crc(buff + 1, retval - 1)); - } - - retval += add; - - if(retval < 0 || retval > buff_sz) - { - memset(buff, ' ', buff_sz); - retval = buff_sz; - } - - va_end(arg_ptr); - - return retval; -} - -/** - * \brief Analyse string (specificate for NMEA sentences) - */ -int nmea_utils_scanf(const char *buff, int buff_sz, const char *format, ...) -{ - const char *beg_tok; - const char *end_buf = buff + buff_sz; - - va_list arg_ptr; - int tok_type = NMEA_TOKS_COMPARE; - int width = 0; - const char *beg_fmt = 0; - int snum = 0, unum = 0; - - int tok_count = 0; - void *parg_target; - - va_start(arg_ptr, format); - - for(; *format && buff < end_buf; ++format) - { - switch(tok_type) - { - case NMEA_TOKS_COMPARE: - if('%' == *format) - tok_type = NMEA_TOKS_PERCENT; - else if(*buff++ != *format) - goto fail; - break; - case NMEA_TOKS_PERCENT: - width = 0; - beg_fmt = format; - tok_type = NMEA_TOKS_WIDTH; - case NMEA_TOKS_WIDTH: - if(isdigit(*format)) - break; - { - tok_type = NMEA_TOKS_TYPE; - if(format > beg_fmt) - width = nmea_utils_atoi(beg_fmt, (int)(format - beg_fmt), 10); - } - case NMEA_TOKS_TYPE: - beg_tok = buff; - - if(!width && ('c' == *format || 'C' == *format) && *buff != format[1]) - width = 1; - - if(width) - { - if(buff + width <= end_buf) - buff += width; - else - goto fail; - } - else - { - if(!format[1] || (0 == (buff = (char *)memchr(buff, format[1], end_buf - buff)))) - buff = end_buf; - } - - if(buff > end_buf) - goto fail; - - tok_type = NMEA_TOKS_COMPARE; - tok_count++; - - parg_target = 0; width = (int)(buff - beg_tok); - - switch(*format) - { - case 'c': - case 'C': - parg_target = (void *)va_arg(arg_ptr, char *); - if(width && 0 != (parg_target)) - *((char *)parg_target) = *beg_tok; - break; - case 's': - case 'S': - parg_target = (void *)va_arg(arg_ptr, char *); - if(width && 0 != (parg_target)) - { - memcpy(parg_target, beg_tok, width); - ((char *)parg_target)[width] = '\0'; - } - break; - case 'f': - case 'g': - case 'G': - case 'e': - case 'E': - parg_target = (void *)va_arg(arg_ptr, double *); - if(width && 0 != (parg_target)) - *((double *)parg_target) = nmea_utils_atof(beg_tok, width); - break; - }; - - if(parg_target) - break; - if(0 == (parg_target = (void *)va_arg(arg_ptr, int *))) - break; - if(!width) - break; - - switch(*format) - { - case 'd': - case 'i': - snum = nmea_utils_atoi(beg_tok, width, 10); - memcpy(parg_target, &snum, sizeof(int)); - break; - case 'u': - unum = nmea_utils_atoi(beg_tok, width, 10); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - case 'x': - case 'X': - unum = nmea_utils_atoi(beg_tok, width, 16); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - case 'o': - unum = nmea_utils_atoi(beg_tok, width, 8); - memcpy(parg_target, &unum, sizeof(unsigned int)); - break; - default: - goto fail; - }; - - break; - }; - } - -fail: - - va_end(arg_ptr); - - return tok_count; -} - - -double nmea_utils_random(double min, double max) -{ - static double rand_max = RAND_MAX; - double rand_val = rand(); - double bounds = max - min; - return min + (rand_val * bounds) / rand_max; -} Index: trunk/firmware/sources/saigd/nmealib++/utils.h =================================================================== diff --git a/trunk/firmware/sources/saigd/nmealib++/utils.h b/trunk/firmware/sources/saigd/nmealib++/utils.h deleted file mode 10644 --- a/trunk/firmware/sources/saigd/nmealib++/utils.h (revision 116) +++ /dev/null (revision 117) @@ -1,38 +0,0 @@ -/* - * utils.h - * - * Created on: Oct 21, 2014 - * Author: Alan Aguilar Sologuren - */ - -#ifndef NMEALIB___UTILS_H_ -#define NMEALIB___UTILS_H_ - -#include "nmea_defs.h" - -double nmea_utils_degree2radian(double val); -double nmea_utils_radian2degree(double val); -double nmea_utils_ndeg2degree(double val); -double nmea_utils_degree2ndeg(double val); -double nmea_utils_ndeg2radian(double val); -double nmea_utils_radian2ndeg(double val); -double nmea_utils_calc_pdop(double hdop, double vdop); -double nmea_utils_dop2meters(double dop); -double nmea_utils_meters2dop(double meters); -double nmea_utils_distance(const nmeaPOS *from_pos, const nmeaPOS *to_pos); -double nmea_utils_distance_ellipsoid(const nmeaPOS *from_pos, const nmeaPOS *to_pos, - double *from_azimuth, double *to_azimuth); -int nmea_utils_move_horz(const nmeaPOS *start_pos, nmeaPOS *end_pos, - double azimuth, double distance); -int nmea_utils_move_horz_ellipsoid(const nmeaPOS *start_pos, nmeaPOS *end_pos, - double azimuth, double distance, double *end_azimuth ); - -int nmea_utils_calc_crc(const char *buff, int buff_sz); -int nmea_utils_atoi(const char *str, int str_sz, int radix); -double nmea_utils_atof(const char *str, int str_sz); -int nmea_utils_printf(char *buff, int buff_sz, const char *format, ...); -int nmea_utils_scanf(const char *buff, int buff_sz, const char *format, ...); - -double nmea_utils_random(double min, double max); - -#endif /* NMEALIB___UTILS_H_ */