이충섭

feat : caculate inclination among all of waypoints and between starting point and current position

......@@ -40,7 +40,7 @@ using math::constrain;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
int static hold=0;
pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
#if !defined(CONSTRAINED_FLASH)
......@@ -1509,19 +1509,83 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
+ vehicle_gps_position.vdop * vehicle_gps_position.vdop),
};
_ekf.setGpsData(gps_msg);
mission_s mission;
dm_read(DM_KEY_MISSION_STATE, 0, &mission, 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));
std::cout<<"target\t"<<i<< "(lat/lon/alt)\t" << mission_item.lat << "\t"<<mission_item.lon<<"\t"<<mission_item.altitude<<std::endl;
if(hold != int(gps_msg.time_usec/1000000))
{
dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
hold=int(gps_msg.time_usec/1000000);
if(hold%2==0)
{
calculate_inclination_target();
print_target_gps();
//print_current_gps(gps_msg);
calculate_inclination_current(gps_msg);
}
}
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
}
}
}
void EKF2::print_current_gps(gps_message gps_msg)
{
std::cout<<"curre\t#"<<mission.current_seq<<"(lat/lon/alt)\t" << gps_msg.lat << "\t"<<gps_msg.lon<<"\t"<<gps_msg.alt<<std::endl;
}
void EKF2::calculate_inclination_target()
{
double x,y;
inclination=new double[int(mission.count)];
for (size_t i = 0; i < mission.count-1; i++) {
struct mission_item_s mission_item {};
struct mission_item_s next_mission_item {};
dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
dm_read((dm_item_t)mission.dataman_id, i+1, &next_mission_item, sizeof(mission_item_s));
//std:: cout << "next : "<<next_mission_item.lat<<std::endl;
//std:: cout << "curr : "<<mission_item.lat<<std::endl;
x=next_mission_item.lat-mission_item.lat;
y=next_mission_item.lon-mission_item.lon;
std::cout.setf(std::ios::fixed);
std::cout.precision(7);
if(x!=0.0){
inclination[i]=y/x;
}
else
{
inclination[i]=0.0;
}
/*if(int(i)==int(mission.current_seq)-1){
std::cout <<"[inclina]#"<<i<<"\t"<<inclination[i]<<std::endl;
}*/
}
}
void EKF2::calculate_inclination_current(gps_message gps_msg)
{
double x,y;
double check_inclination=0;
x=target_lat-gps_msg.lat;
y=target_lon-gps_msg.lon;
if(x!=0.0){
check_inclination=y/x;
}
std::cout.setf(std::ios::fixed);
std::cout.precision(7);
std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
}
void EKF2::print_target_gps()
{
std::cout.unsetf(std::ios::fixed);
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
if(int(mission.current_seq)==int(i)){
dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
target_lat=int32_t(mission_item.lat * long(pow(10,7)));
target_lon=int32_t(mission_item.lon * long(pow(10,7)));
target_alt=int32_t(mission_item.altitude * long(pow(10,4)));
//std::cout<<"start\t#"<<i<< "(lat/lon/alt)\t" << target_lat<< "\t"<<target_lon<<"\t"<<target_alt<<std::endl;
}
}
}
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
{
const unsigned last_generation = _magnetometer_sub.get_last_generation();
......
......@@ -43,7 +43,7 @@
#include "EKF2Selector.hpp"
#include <float.h>
#include <dataman/dataman.h>
#include <containers/LockGuard.hpp>
#include <drivers/drv_hrt.h>
#include <lib/ecl/EKF/ekf.h>
......@@ -155,14 +155,19 @@ private:
void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
void UpdateMagCalibration(const hrt_abstime &timestamp);
void print_target_gps();
void print_current_gps(gps_message gps_msg);
void calculate_inclination_target();
void calculate_inclination_current(gps_message gps_msg);
double* inclination=nullptr;
int32_t target_lat,target_lon,target_alt;
mission_s mission;
/*
* Calculate filtered WGS84 height from estimated AMSL height
*/
float filter_altitude_ellipsoid(float amsl_hgt);
static constexpr float sq(float x) { return x * x; };
const bool _replay_mode{false}; ///< true when we use replay data from a log
const bool _multi_mode;
int _instance{0};
......@@ -176,6 +181,8 @@ private:
perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
perf_counter_t _imu_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
perf_counter_t _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag message missed")};
// Used to check, save and use learned magnetometer biases
hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)
......