Skip to content
Snippets Groups Projects
Commit a3294f76 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

bug found, still implementing copyArrayElement specialization

parent 5839bd6e
No related branches found
No related tags found
2 merge requests!20new tag,!19new tag
Hide whitespace changes
Inline Side-by-side
include/gnss_utils/navigation.h
+ 126
− 7
View file @ a3294f76
......@@ -69,11 +69,13 @@ class Navigation
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
static bool addToArray(const T &new_element, T *array, int &n, int &nmax);
static void copyArrayElement(const T &new_element, T& element_spot);
template <typename T>
static bool copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out);
static bool addToArray(const T &new_element, T *&array, int &n, int &nmax);
template <typename T>
static void freeArray(T *array, int &n, int &nmax);
static bool copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out);
template <typename T>
static void freeArray(T *&array, int &n, int &nmax);
static void freeEph(nav_t &nav);
static void freeGeph(nav_t &nav);
static void freeSeph(nav_t &nav);
......@@ -251,12 +253,114 @@ inline void Navigation::freeAlmanac()
//////////////////////////////// nav UTILS //////////////////////////////////////
template <typename T>
bool Navigation::addToArray(const T &new_element, T *array, int &n, int &nmax)
inline void Navigation::copyArrayElement(const T &new_element, T& element_spot)
{
element_spot = new_element;
}
//template <>
//inline void Navigation::copyArrayElement<eph_t>(const eph_t& new_element, eph_t& element_spot)
//{
// element_spot = new_element;
// std::cout << "tgd new element " << new_element.tgd << std::endl;
// std::cout << "before tgd element_spot " << element_spot.tgd << std::endl;
// double tgd[4];
// element_spot.tgd = tgd;
// std::cout << "tgd element_spot " << element_spot.tgd << std::endl;
//}
//template <>
//inline void Navigation::copyArrayElement<geph_t>(const geph_t& new_element, geph_t& element_spot)
//{
// element_spot = new_element;
// std::cout << "pos new element " << new_element.pos << std::endl;
// std::cout << "before pos element_spot " << element_spot.pos << std::endl;
// double pos[3];
// element_spot.pos = pos;
// std::cout << "pos element_spot " << element_spot.pos << std::endl;
//}
//template <>
//inline void Navigation::copyArrayElement<seph_t>(const seph_t& new_element, seph_t& element_spot)
//{
// element_spot = new_element;
// std::cout << "pos new element " << new_element.pos << std::endl;
// std::cout << "before pos element_spot " << element_spot.pos << std::endl;
// double pos[3];
// element_spot.pos = pos;
// std::cout << "pos element_spot " << element_spot.pos << std::endl;
//}
//template <>
//inline void Navigation::copyArrayElement<peph_t>(const peph_t& new_element, peph_t& element_spot)
//{
// element_spot = new_element;
// std::cout << "pos new element " << new_element.pos << std::endl;
// std::cout << "before pos element_spot " << element_spot.pos << std::endl;
// double pos[MAXSAT][4];
// element_spot.pos = pos;
// std::cout << "pos element_spot " << element_spot.pos << std::endl;
//}
//template <>
//inline void Navigation::copyArrayElement<pclk_t>(const pclk_t& new_element, pclk_t& element_spot)
//{
// element_spot = new_element;
// std::cout << "clk new element " << new_element.clk << std::endl;
// std::cout << "before clk element_spot " << element_spot.clk << std::endl;
// double clk[MAXSAT][1];
// element_spot.clk = clk;
// std::cout << "clk element_spot " << element_spot.clk << std::endl;
//}
template <>
inline void Navigation::copyArrayElement<alm_t>(const alm_t& new_element, alm_t& element_spot)
{
std::cout << "copyArrayElement<alm_t>\n";
std::cout << "new element: ";
std::cout << new_element.sat << std::endl; /* satellite number */
std::cout << new_element.svh << std::endl; /* sv health (0:ok) */
std::cout << new_element.svconf << std::endl; /* as and sv config */
std::cout << new_element.week << std::endl; /* GPS/QZS: gps week, GAL: galileo week */
//toa; /* Toa */
std::cout << new_element.A << std::endl;
std::cout << new_element.e << std::endl;
std::cout << new_element.i0 << std::endl;
std::cout << new_element.OMG0 << std::endl;
std::cout << new_element.omg << std::endl;
std::cout << new_element.M0 << std::endl;
std::cout << new_element.OMGd << std::endl;
std::cout << new_element.toas << std::endl; /* Toa (s) in week */
std::cout << new_element.f0 << std::endl;
std::cout << new_element.f1 << std::endl;
std::cout << "element_spot: " << std::endl;
std::cout << element_spot.sat << std::endl; /* satellite number */
std::cout << element_spot.svh << std::endl; /* sv health (0:ok) */
std::cout << element_spot.svconf << std::endl; /* as and sv config */
std::cout << element_spot.week << std::endl; /* GPS/QZS: gps week, GAL: galileo week */
//toa; /* Toa */
std::cout << element_spot.A << std::endl;
std::cout << element_spot.e << std::endl;
std::cout << element_spot.i0 << std::endl;
std::cout << element_spot.OMG0 << std::endl;
std::cout << element_spot.omg << std::endl;
std::cout << element_spot.M0 << std::endl;
std::cout << element_spot.OMGd << std::endl;
std::cout << element_spot.toas << std::endl; /* Toa (s) in week */
std::cout << element_spot.f0 << std::endl;
std::cout << element_spot.f1 << std::endl;
std::cout << "copying..";
element_spot = new_element;
}
template <typename T>
inline bool Navigation::addToArray(const T &new_element, T *&array, int &n, int &nmax)
{
std::cout << "addToArray: n = " << n << " nmax = " << nmax << "\n";
// "inspired" from RTKLIB rinex.c
T *array_ref;
if (nmax <= n)
{
std::cout << "addToArray: nmax <= n\n";
nmax += 1024;
if (!(array_ref = (T*)realloc(array, sizeof(T) * nmax)))
{
......@@ -266,27 +370,42 @@ bool Navigation::addToArray(const T &new_element, T *array, int &n, int &nmax)
n = nmax = 0;
return false;
}
std::cout << "addToArray: assigning reallocated array\n";
array = array_ref;
}
array[n++] = new_element;
std::cout << "addToArray: adding element " << n << "\n";
copyArrayElement<T>(new_element, array[n++]);
std::cout << "addToArray: added!\n";
return true;
}
template<typename T>
bool Navigation::copyArray(const T *array_in, const int &n_in, T *array_out, int &n_out, int &nmax_out)
inline bool Navigation::copyArray(const T *array_in, const int &n_in, T *&array_out, int &n_out, int &nmax_out)
{
std::cout << "copyArray: " << n_in << " elements\n";
if (array_in == NULL)
return false;
std::cout << "copyArray: array in not null\n";
for (int i = 0; i<n_in; i++)
{
std::cout << "copyArray: adding element " << i << "\n";
if (!addToArray<T>(array_in[i], array_out, n_out, nmax_out))
{
std::cout << "copyArray: failed to add..\n";
return false;
}
std::cout << "copyArray: n_out = " << n_out << " nmax_out = " << nmax_out << "\n";
}
std::cout << "copyArray: all copied\n";
return true;
}
template<typename T>
void Navigation::freeArray(T *array, int &n, int &nmax)
void Navigation::freeArray(T *&array, int &n, int &nmax)
{
if (array != NULL)
free(array);
......
src/ublox_raw.cpp
+ 60
− 42
View file @ a3294f76
......@@ -21,50 +21,68 @@ RawDataType UBloxRaw::addDataStream(const std::vector<u_int8_t>& data_stream)
{
// Update type based on RTKLIB
for (auto data_byte = data_stream.begin(); data_byte != data_stream.end(); ++data_byte)
{
raw_data_type_ = input_ubx(&raw_data_, (unsigned char)*data_byte);
switch (raw_data_type_)
{
case OBS: // Observations
updateObservations();
break;
case NAV_EPH: // Ephemeris
nav_.copyEphemeris(raw_data_.nav);
break;
case NAV_SBAS: // SBAS
nav_.addSbasMessage(raw_data_.sbsmsg);
break;
case NAV_ALM: // Almanac and ion/utc parameters
nav_.freeAlmanac();
nav_.copyAlmanac(raw_data_.nav);
nav_.copyIonUtc(raw_data_.nav);
break;
// Not handled messages
case NAV_ANT:
std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
break;
case NAV_DGPS:
std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
break;
case NAV_SSR:
std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
break;
case NAV_LEX:
std::cout << "UBloxRaw: Received lex message. Not handled.\n";
break;
case ERROR:
std::cout << "UBloxRaw: Received error message. Not handled.\n";
break;
default:
std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
break;
}
return raw_data_type_;
switch (raw_data_type_)
{
case NO: //
break;
case OBS: // Observations
std::cout << "Observations received!\n";
updateObservations();
std::cout << "Observations updated!\n";
break;
case NAV_EPH: // Ephemeris
std::cout << "Ephemeris received!\n";
nav_.copyEphemeris(raw_data_.nav);
std::cout << "Ephemeris copied!\n";
break;
case NAV_SBAS: // SBAS
std::cout << "SBAS received!\n";
nav_.addSbasMessage(raw_data_.sbsmsg);
std::cout << "SBAS added!\n";
break;
case NAV_ALM: // Almanac and ion/utc parameters
std::cout << "Almanac and ion/utc parameters received!\n";
nav_.freeAlmanac();
std::cout << "Almanac freed!\n";
nav_.copyAlmanac(raw_data_.nav);
std::cout << "Almanac copied!\n";
nav_.copyIonUtc(raw_data_.nav);
std::cout << "ION UTC copied!\n";
break;
// Not handled messages
case NAV_ANT:
std::cout << "UBloxRaw: Received antenna postion parameters. Not handled.\n";
break;
case NAV_DGPS:
std::cout << "UBloxRaw: Received dgps correction. Not handled.\n";
break;
case NAV_SSR:
std::cout << "UBloxRaw: Received ssr message. Not handled.\n";
break;
case NAV_LEX:
std::cout << "UBloxRaw: Received lex message. Not handled.\n";
break;
case ERROR:
std::cout << "UBloxRaw: Received error message. Not handled.\n";
break;
default:
std::cout << "UBloxRaw: Received unknown message. Not handled.\n";
break;
}
}
std::cout << "ALL BYTES DECODED!\n\n";
return raw_data_type_;
}
void UBloxRaw::updateObservations()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment