이재용

upload get_mission code that need to print one time

......@@ -40,11 +40,13 @@
*/
#include "ekf.h"
#include <iostream>
#include <ecl.h>
#include <geo_lookup/geo_mag_declination.h>
#include <mathlib/mathlib.h>
#include <modules/navigator/mission.h>
static int hold=0;
int flag=0;
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
#define MASK_GPS_PDOP (1<<1)
......@@ -58,6 +60,26 @@
bool Ekf::collect_gps(const gps_message &gps)
{
mission_s mission;
(dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s));
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s));
if(flag==0)
{
/* error reading, mission is invalid */
std::cout<<"----------"<<std::endl;
std::cout<<i+1<<std::endl;
std::cout<< mission_item.lat <<std::endl;
}
}
flag=1;
// Run GPS checks always
_gps_checks_passed = gps_is_good(gps);
......@@ -140,6 +162,19 @@ bool Ekf::collect_gps(const gps_message &gps)
_earth_rate_NED = calcEarthRateNED((float)math::radians(lat));
}
}
if(hold != int(gps.time_usec/1000000))
{
hold=int(gps.time_usec/1000000);
if(hold%2==0)
{
std::cout <<"----------------------current--------------------------"<<std::endl;
std :: cout << "time : "<<gps.time_usec/1000000<<std::endl;
std :: cout << "lat : " <<gps.lat <<std::endl;
std :: cout << "lon : " <<gps.lon <<std::endl;
std :: cout << "alt : " <<gps.alt <<std::endl;
}
}
// print GPS data every 1 seocnds
// start collecting GPS if there is a 3D fix and the NED origin has been set
return _NED_origin_initialised && (gps.fix_type >= 3);
......