xyz geolocation::toxyz() const
{
// Approach developed by:
//
// (Olson, D.K. (1996).
// "Converting earth-Centered, Earth-Fixed Coordinates to Geodetic Coordinates,"
// IEEE Transactions on Aerospace and Electronic Systems, Vol. 32, No. 1, January 1996, pp. 473 - 476).
//
//
// Java implementation:
//
// D. Rose (2014).
// "Converting between Earth-Centered, Earth Fixed and Geodetic Coordinates"
//
//Convert Lat, Lon, Altitude to Earth-Centered-Earth-Fixed (ECEF)
//Input is a three element array containing lat, lon (rads) and alt (m)
//Returned array contains x, y, z in meters
static const double a = 6378137.0; //WGS-84 semi-major axis
static const double e2 = 6.6943799901377997e-3; //WGS-84 first eccentricity squared
double lat = degreestoradians(Lat());
double lon = degreestoradians(Long());
double alt = Alt();
double n = a / sqrt(1 - e2*sin(lat)*sin(lat));
double dx = ((n + alt)*cos(lat)*cos(lon)); //ECEF x
double dy = ((n + alt)*cos(lat)*sin(lon)); //ECEF y
double dz = ((n*(1 - e2) + alt)*sin(lat)); //ECEF z
return(xyz(dx, dy, dz)); //Return x, y, z in ECEF
}
void ChoiceTable::ForestList(int input_vertex,int reference_vertex,TreeGraphPtr T1, TreeGraphPtr T2, MatchRecordList& sequence)
{
ChoiceList* L=getList(input_vertex,reference_vertex);
int forest_choice=Lat(L,2);
switch(forest_choice)
{
case 1: ForestList(Lat(L,3),reference_vertex,T1,T2,sequence);break;
case 2: ForestList(input_vertex,Lat(L,3),T1,T2,sequence);break;
case 3:
{
for (int i=0;i<T1->getNbChild(input_vertex);i++)
{
int i_node=T1->child(input_vertex,i);
int r_node=Lat(L,3+i);
if (r_node!=-1) TreeList(i_node,r_node,T1,T2,sequence);
}
}
break;
default : break;
}
}
void ChoiceTable::TreeList(int input_vertex, int reference_vertex, TreeGraphPtr T1, TreeGraphPtr T2, MatchRecordList& sequence){
if ((!T1->isNull())&&(!T2->isNull()))
{
ChoiceList* L=getList(input_vertex,reference_vertex);
int tree_choice=L->front();
switch(tree_choice)
{
case 1:
{
TreeList(Lat(L,1),reference_vertex,T1,T2,sequence);
}
break;
case 2:
{
TreeList(input_vertex,Lat(L,1),T1,T2,sequence);
}
break;
case 3:
{
sequence.push_back(MatchRecord(input_vertex,reference_vertex));
ForestList(input_vertex,reference_vertex,T1,T2,sequence);
}
break;
case 4:
{
int size = Lat(L,1);
int nbChoices = L->size();
/*
Fred temptative of fix:
The actual list of id of matched elements seems to be the n last value of the choice list.
*/
int input_vertex;
for (int i=0;i<size;i++){
input_vertex = Lat(L,nbChoices-size+i);
sequence.push_back(MatchRecord(input_vertex,reference_vertex));
}
// question : is input_vertex to continue the fist or the last of the list. I choose the first
input_vertex = Lat(L,nbChoices-size);
ForestList(input_vertex,reference_vertex,T1,T2,sequence);
}
break;
case 5:
{
int size = Lat(L,1);
int nbChoices = L->size();
/*
Fred temptative of fix:
The actual list of id of matched elements seems to be the n last value of the choice list.
*/
int reference_vertex;
for (int i=0;i<size;i++){
reference_vertex = Lat(L,nbChoices-size+i);
sequence.push_back(MatchRecord(input_vertex,reference_vertex));
}
// question : is reference_vertex to continue the fist or the last of the list. I choose the first
reference_vertex = Lat(L,nbChoices-size);
ForestList(input_vertex,reference_vertex,T1,T2,sequence);
}
break;
default : break;
}
}
}
/** Periodic function
*/
void meteo_stick_periodic(void)
{
// Read ADC
#ifdef MS_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.pressure);
#endif
#ifdef MS_DIFF_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.diff_pressure);
#endif
#ifdef MS_TEMPERATURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.temperature);
#endif
// Read PWM
#ifdef MS_HUMIDITY_PWM_INPUT
meteo_stick.humidity_period = pwm_input_period_tics[MS_HUMIDITY_PWM_INPUT];
meteo_stick.current_humidity = get_humidity(meteo_stick.humidity_period);
#endif
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Extract calibration data
if (!mtostk_populate_cal_from_buffer(&meteo_stick.calib, (uint8_t *)(meteo_stick.eeprom.rx_buf + 3))) {
// Extraction failed
// Force number of calibration to 0 for all sensors
int i;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
meteo_stick.calib.params[i].num_temp = 0;
}
}
} else if (meteo_stick.eeprom.spi_trans.status == SPITransDone) {
// Load reading request (reading 1Kb from address 0x0)
eeprom25AA256_read(&meteo_stick.eeprom, 0x0, 1024);
}
#endif
// Log data
#if LOG_MS
if (pprzLogFile != -1) {
if (!log_ptu_started) {
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Print calibration data in the log header
sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid);
int i, j, k;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %d, num_temp: %d, num_coeff: %d\n", i,
meteo_stick.calib.params[i].timestamp,
meteo_stick.calib.params[i].num_temp,
meteo_stick.calib.params[i].num_coeff);
if (meteo_stick.calib.params[i].timestamp == 0) {
continue; // No calibration
}
for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) {
sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]);
sdLogWriteLog(pprzLogFile, "# Coeffs:");
for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) {
sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]);
}
sdLogWriteLog(pprzLogFile, "\n");
}
}
sdLogWriteLog(pprzLogFile, "#\n");
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
}
#else
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
#endif
} else {
