이충섭

fix : print target gps fucntion by using dataman in ekf2

......@@ -32,7 +32,9 @@
****************************************************************************/
#include "EKF2.hpp"
#include <iostream>
#include <px4_platform_common/defines.h>
#include <dataman/dataman.h>
using namespace time_literals;
using math::constrain;
using matrix::Eulerf;
......@@ -44,7 +46,6 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
#if !defined(CONSTRAINED_FLASH)
static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
#endif // !CONSTRAINED_FLASH
EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, config),
......@@ -169,6 +170,8 @@ EKF2::~EKF2()
{
perf_free(_ecl_ekf_update_perf);
perf_free(_ecl_ekf_update_full_perf);
perf_free(_imu_missed_perf);
perf_free(_mag_missed_perf);
}
bool EKF2::multi_init(int imu, int mag)
......@@ -193,17 +196,18 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_visual_odometry_aligned_pub.advertised();
_yaw_est_pub.advertise();
_vehicle_imu_sub.ChangeInstance(imu);
_magnetometer_sub.ChangeInstance(mag);
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
const int status_instance = _estimator_states_pub.get_instance();
if ((status_instance >= 0)
if ((status_instance >= 0) && changed_instance
&& (_attitude_pub.get_instance() == status_instance)
&& (_local_position_pub.get_instance() == status_instance)
&& (_global_position_pub.get_instance() == status_instance)) {
_instance = status_instance;
ScheduleNow();
return true;
}
......@@ -219,6 +223,12 @@ int EKF2::print_status()
_ekf.local_position_is_valid(), _ekf.global_position_is_valid());
perf_print_counter(_ecl_ekf_update_perf);
perf_print_counter(_ecl_ekf_update_full_perf);
perf_print_counter(_imu_missed_perf);
if (_device_id_mag != 0) {
perf_print_counter(_mag_missed_perf);
}
return 0;
}
......@@ -298,8 +308,9 @@ void EKF2::Run()
imu_updated = _vehicle_imu_sub.update(&imu);
if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
_vehicle_imu_sub.get_last_generation());
perf_count(_imu_missed_perf);
PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
_vehicle_imu_sub.get_last_generation());
}
imu_sample_new.time_us = imu.timestamp_sample;
......@@ -1498,7 +1509,13 @@ 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;
}
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
}
......@@ -1513,8 +1530,9 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
if (_magnetometer_sub.update(&magnetometer)) {
if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
_magnetometer_sub.get_last_generation());
perf_count(_mag_missed_perf);
PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
_magnetometer_sub.get_last_generation());
}
bool reset = false;
......@@ -1738,7 +1756,8 @@ int EKF2::task_spawn(int argc, char *argv[])
while ((multi_instances_allocated < multi_instances)
&& (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
&& (hrt_elapsed_time(&time_started) < 30_s)) {
&& ((hrt_elapsed_time(&time_started) < 30_s)
|| (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
vehicle_status_sub.update();
......@@ -1763,16 +1782,21 @@ int EKF2::task_spawn(int argc, char *argv[])
if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
_objects[actual_instance].store(ekf2_inst);
ekf2_inst->ScheduleNow();
success = true;
multi_instances_allocated++;
ekf2_instance_created[imu][mag] = true;
if (actual_instance == 0) {
// force selector to run immediately if first instance started
_ekf2_selector.load()->ScheduleNow();
}
PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
_ekf2_selector.load()->ScheduleNow();
// sleep briefly before starting more instances
px4_usleep(10000);
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
......
......@@ -47,7 +47,7 @@
#include "mission.h"
#include "navigator.h"
#include <iostream>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
......@@ -381,6 +381,7 @@ Mission::set_execution_mode(const uint8_t mode)
_mission_execution_mode = mode;
}
}
bool
Mission::find_mission_land_start()
{
......@@ -620,12 +621,11 @@ Mission::set_mission_items()
struct mission_item_s mission_item_after_next_position;
bool has_next_position_item = false;
bool has_after_next_position_item = false;
work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
&mission_item_after_next_position, &has_after_next_position_item)) {
print_next_mission(&mission_item_next_position);
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_MISSION) {
mavlink_log_info(_navigator->get_mavlink_log_pub(),
......@@ -633,6 +633,7 @@ Mission::set_mission_items()
"Executing Mission");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_MISSION;
} else {
......@@ -1436,14 +1437,17 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
offset = -1;
}
if (read_mission_item(0, mission_item)) {
first_res = true;
/* trying to find next position mission item */
while (read_mission_item(offset, next_position_mission_item)) {
if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\
offset--;
}
else {
if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
offset--;
} else {
offset++;
}
......@@ -1452,11 +1456,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
break;
}
}
if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
after_next_position_mission_item && has_after_next_position_item) {
/* trying to find next next position mission item */
while (read_mission_item(offset, after_next_position_mission_item)) {
offset++;
if (item_contains_position(*after_next_position_mission_item)) {
*has_after_next_position_item = true;
break;
......@@ -1467,24 +1473,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
return first_res;
}
bool
Mission::print_next_mission(struct mission_item_s *mis)
{
std::cout <<"----------------------target--------------------------"<<std::endl;
std::cout << "lat : "<<mis->lat <<std:: endl;
std::cout << "lon : "<<mis->lon <<std:: endl;
std::cout << "alt : "<<mis->altitude <<std:: endl;
//std::cout << "[whatever] "<<mis->timestamp <<std:: endl;
mission_index++;
return 0;
}
bool
Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
{
/* select mission */
const int current_index = _current_mission_index;
int index_to_read = current_index + offset;
int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read;
const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
......@@ -1564,6 +1560,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
return true;
}
}
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.");
return false;
......@@ -1865,5 +1862,6 @@ void Mission::publish_navigator_mission_item()
navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
navigator_mission_item.timestamp = hrt_absolute_time();
_navigator_mission_item_pub.publish(navigator_mission_item);
}
......
......@@ -49,7 +49,7 @@
#include "navigator_mode.h"
#include <float.h>
#include <iostream>
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
......@@ -227,7 +227,7 @@ private:
* Find and store the index of the landing sequence (DO_LAND_START)
*/
bool find_mission_land_start();
bool print_next_mission(struct mission_item_s *mis);
/**
* Return the index of the closest mission item to the current global position.
*/
......@@ -269,7 +269,7 @@ private:
MISSION_TYPE_NONE,
MISSION_TYPE_MISSION
} _mission_type{MISSION_TYPE_NONE};
int mission_index=-1;
bool _inited{false};
bool _home_inited{false};
bool _need_mission_reset{false};
......