이충섭

fix : print target gps fucntion by using dataman in ekf2

...@@ -32,7 +32,9 @@ ...@@ -32,7 +32,9 @@
32 ****************************************************************************/ 32 ****************************************************************************/
33 33
34 #include "EKF2.hpp" 34 #include "EKF2.hpp"
35 - 35 +#include <iostream>
36 +#include <px4_platform_common/defines.h>
37 +#include <dataman/dataman.h>
36 using namespace time_literals; 38 using namespace time_literals;
37 using math::constrain; 39 using math::constrain;
38 using matrix::Eulerf; 40 using matrix::Eulerf;
...@@ -44,7 +46,6 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {}; ...@@ -44,7 +46,6 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
44 #if !defined(CONSTRAINED_FLASH) 46 #if !defined(CONSTRAINED_FLASH)
45 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr}; 47 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
46 #endif // !CONSTRAINED_FLASH 48 #endif // !CONSTRAINED_FLASH
47 -
48 EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): 49 EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
49 ModuleParams(nullptr), 50 ModuleParams(nullptr),
50 ScheduledWorkItem(MODULE_NAME, config), 51 ScheduledWorkItem(MODULE_NAME, config),
...@@ -169,6 +170,8 @@ EKF2::~EKF2() ...@@ -169,6 +170,8 @@ EKF2::~EKF2()
169 { 170 {
170 perf_free(_ecl_ekf_update_perf); 171 perf_free(_ecl_ekf_update_perf);
171 perf_free(_ecl_ekf_update_full_perf); 172 perf_free(_ecl_ekf_update_full_perf);
173 + perf_free(_imu_missed_perf);
174 + perf_free(_mag_missed_perf);
172 } 175 }
173 176
174 bool EKF2::multi_init(int imu, int mag) 177 bool EKF2::multi_init(int imu, int mag)
...@@ -193,17 +196,18 @@ bool EKF2::multi_init(int imu, int mag) ...@@ -193,17 +196,18 @@ bool EKF2::multi_init(int imu, int mag)
193 _estimator_visual_odometry_aligned_pub.advertised(); 196 _estimator_visual_odometry_aligned_pub.advertised();
194 _yaw_est_pub.advertise(); 197 _yaw_est_pub.advertise();
195 198
196 - _vehicle_imu_sub.ChangeInstance(imu); 199 + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
197 - _magnetometer_sub.ChangeInstance(mag);
198 200
199 const int status_instance = _estimator_states_pub.get_instance(); 201 const int status_instance = _estimator_states_pub.get_instance();
200 202
201 - if ((status_instance >= 0) 203 + if ((status_instance >= 0) && changed_instance
202 && (_attitude_pub.get_instance() == status_instance) 204 && (_attitude_pub.get_instance() == status_instance)
203 && (_local_position_pub.get_instance() == status_instance) 205 && (_local_position_pub.get_instance() == status_instance)
204 && (_global_position_pub.get_instance() == status_instance)) { 206 && (_global_position_pub.get_instance() == status_instance)) {
205 207
206 _instance = status_instance; 208 _instance = status_instance;
209 +
210 + ScheduleNow();
207 return true; 211 return true;
208 } 212 }
209 213
...@@ -219,6 +223,12 @@ int EKF2::print_status() ...@@ -219,6 +223,12 @@ int EKF2::print_status()
219 _ekf.local_position_is_valid(), _ekf.global_position_is_valid()); 223 _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
220 perf_print_counter(_ecl_ekf_update_perf); 224 perf_print_counter(_ecl_ekf_update_perf);
221 perf_print_counter(_ecl_ekf_update_full_perf); 225 perf_print_counter(_ecl_ekf_update_full_perf);
226 + perf_print_counter(_imu_missed_perf);
227 +
228 + if (_device_id_mag != 0) {
229 + perf_print_counter(_mag_missed_perf);
230 + }
231 +
222 return 0; 232 return 0;
223 } 233 }
224 234
...@@ -298,8 +308,9 @@ void EKF2::Run() ...@@ -298,8 +308,9 @@ void EKF2::Run()
298 imu_updated = _vehicle_imu_sub.update(&imu); 308 imu_updated = _vehicle_imu_sub.update(&imu);
299 309
300 if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { 310 if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
301 - PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation, 311 + perf_count(_imu_missed_perf);
302 - _vehicle_imu_sub.get_last_generation()); 312 + PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
313 + _vehicle_imu_sub.get_last_generation());
303 } 314 }
304 315
305 imu_sample_new.time_us = imu.timestamp_sample; 316 imu_sample_new.time_us = imu.timestamp_sample;
...@@ -1498,7 +1509,13 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) ...@@ -1498,7 +1509,13 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
1498 + vehicle_gps_position.vdop * vehicle_gps_position.vdop), 1509 + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
1499 }; 1510 };
1500 _ekf.setGpsData(gps_msg); 1511 _ekf.setGpsData(gps_msg);
1501 - 1512 + mission_s mission;
1513 + dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
1514 + for (size_t i = 0; i < mission.count; i++) {
1515 + struct mission_item_s mission_item {};
1516 + dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
1517 + std::cout<<"target\t"<<i<< "(lat/lon/alt)\t" << mission_item.lat << "\t"<<mission_item.lon<<"\t"<<mission_item.altitude<<std::endl;
1518 + }
1502 _gps_time_usec = gps_msg.time_usec; 1519 _gps_time_usec = gps_msg.time_usec;
1503 _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid; 1520 _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
1504 } 1521 }
...@@ -1513,8 +1530,9 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) ...@@ -1513,8 +1530,9 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
1513 if (_magnetometer_sub.update(&magnetometer)) { 1530 if (_magnetometer_sub.update(&magnetometer)) {
1514 1531
1515 if (_magnetometer_sub.get_last_generation() != last_generation + 1) { 1532 if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
1516 - PX4_ERR("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation, 1533 + perf_count(_mag_missed_perf);
1517 - _magnetometer_sub.get_last_generation()); 1534 + PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
1535 + _magnetometer_sub.get_last_generation());
1518 } 1536 }
1519 1537
1520 bool reset = false; 1538 bool reset = false;
...@@ -1738,7 +1756,8 @@ int EKF2::task_spawn(int argc, char *argv[]) ...@@ -1738,7 +1756,8 @@ int EKF2::task_spawn(int argc, char *argv[])
1738 1756
1739 while ((multi_instances_allocated < multi_instances) 1757 while ((multi_instances_allocated < multi_instances)
1740 && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) 1758 && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
1741 - && (hrt_elapsed_time(&time_started) < 30_s)) { 1759 + && ((hrt_elapsed_time(&time_started) < 30_s)
1760 + || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
1742 1761
1743 vehicle_status_sub.update(); 1762 vehicle_status_sub.update();
1744 1763
...@@ -1763,16 +1782,21 @@ int EKF2::task_spawn(int argc, char *argv[]) ...@@ -1763,16 +1782,21 @@ int EKF2::task_spawn(int argc, char *argv[])
1763 1782
1764 if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) { 1783 if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
1765 _objects[actual_instance].store(ekf2_inst); 1784 _objects[actual_instance].store(ekf2_inst);
1766 - ekf2_inst->ScheduleNow();
1767 success = true; 1785 success = true;
1768 multi_instances_allocated++; 1786 multi_instances_allocated++;
1769 ekf2_instance_created[imu][mag] = true; 1787 ekf2_instance_created[imu][mag] = true;
1770 1788
1789 + if (actual_instance == 0) {
1790 + // force selector to run immediately if first instance started
1791 + _ekf2_selector.load()->ScheduleNow();
1792 + }
1793 +
1771 PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance, 1794 PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
1772 imu, vehicle_imu_sub.get().accel_device_id, 1795 imu, vehicle_imu_sub.get().accel_device_id,
1773 mag, vehicle_mag_sub.get().device_id); 1796 mag, vehicle_mag_sub.get().device_id);
1774 1797
1775 - _ekf2_selector.load()->ScheduleNow(); 1798 + // sleep briefly before starting more instances
1799 + px4_usleep(10000);
1776 1800
1777 } else { 1801 } else {
1778 PX4_ERR("instance numbering problem instance: %d", actual_instance); 1802 PX4_ERR("instance numbering problem instance: %d", actual_instance);
......
...@@ -47,7 +47,7 @@ ...@@ -47,7 +47,7 @@
47 47
48 #include "mission.h" 48 #include "mission.h"
49 #include "navigator.h" 49 #include "navigator.h"
50 -#include <iostream> 50 +
51 #include <string.h> 51 #include <string.h>
52 #include <drivers/drv_hrt.h> 52 #include <drivers/drv_hrt.h>
53 #include <dataman/dataman.h> 53 #include <dataman/dataman.h>
...@@ -381,6 +381,7 @@ Mission::set_execution_mode(const uint8_t mode) ...@@ -381,6 +381,7 @@ Mission::set_execution_mode(const uint8_t mode)
381 _mission_execution_mode = mode; 381 _mission_execution_mode = mode;
382 } 382 }
383 } 383 }
384 +
384 bool 385 bool
385 Mission::find_mission_land_start() 386 Mission::find_mission_land_start()
386 { 387 {
...@@ -620,12 +621,11 @@ Mission::set_mission_items() ...@@ -620,12 +621,11 @@ Mission::set_mission_items()
620 struct mission_item_s mission_item_after_next_position; 621 struct mission_item_s mission_item_after_next_position;
621 bool has_next_position_item = false; 622 bool has_next_position_item = false;
622 bool has_after_next_position_item = false; 623 bool has_after_next_position_item = false;
624 +
623 work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; 625 work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
624 626
625 if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, 627 if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
626 &mission_item_after_next_position, &has_after_next_position_item)) { 628 &mission_item_after_next_position, &has_after_next_position_item)) {
627 - print_next_mission(&mission_item_next_position);
628 -
629 /* if mission type changed, notify */ 629 /* if mission type changed, notify */
630 if (_mission_type != MISSION_TYPE_MISSION) { 630 if (_mission_type != MISSION_TYPE_MISSION) {
631 mavlink_log_info(_navigator->get_mavlink_log_pub(), 631 mavlink_log_info(_navigator->get_mavlink_log_pub(),
...@@ -633,6 +633,7 @@ Mission::set_mission_items() ...@@ -633,6 +633,7 @@ Mission::set_mission_items()
633 "Executing Mission"); 633 "Executing Mission");
634 user_feedback_done = true; 634 user_feedback_done = true;
635 } 635 }
636 +
636 _mission_type = MISSION_TYPE_MISSION; 637 _mission_type = MISSION_TYPE_MISSION;
637 638
638 } else { 639 } else {
...@@ -1436,14 +1437,17 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1436,14 +1437,17 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1436 if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { 1437 if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
1437 offset = -1; 1438 offset = -1;
1438 } 1439 }
1440 +
1439 if (read_mission_item(0, mission_item)) { 1441 if (read_mission_item(0, mission_item)) {
1442 +
1440 first_res = true; 1443 first_res = true;
1444 +
1441 /* trying to find next position mission item */ 1445 /* trying to find next position mission item */
1442 while (read_mission_item(offset, next_position_mission_item)) { 1446 while (read_mission_item(offset, next_position_mission_item)) {
1443 - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\ 1447 + if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
1444 - offset--; 1448 + offset--;
1445 - } 1449 +
1446 - else { 1450 + } else {
1447 offset++; 1451 offset++;
1448 } 1452 }
1449 1453
...@@ -1452,11 +1456,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1452,11 +1456,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1452 break; 1456 break;
1453 } 1457 }
1454 } 1458 }
1459 +
1455 if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && 1460 if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
1456 after_next_position_mission_item && has_after_next_position_item) { 1461 after_next_position_mission_item && has_after_next_position_item) {
1457 /* trying to find next next position mission item */ 1462 /* trying to find next next position mission item */
1458 while (read_mission_item(offset, after_next_position_mission_item)) { 1463 while (read_mission_item(offset, after_next_position_mission_item)) {
1459 offset++; 1464 offset++;
1465 +
1460 if (item_contains_position(*after_next_position_mission_item)) { 1466 if (item_contains_position(*after_next_position_mission_item)) {
1461 *has_after_next_position_item = true; 1467 *has_after_next_position_item = true;
1462 break; 1468 break;
...@@ -1467,24 +1473,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item, ...@@ -1467,24 +1473,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
1467 1473
1468 return first_res; 1474 return first_res;
1469 } 1475 }
1470 -bool
1471 -Mission::print_next_mission(struct mission_item_s *mis)
1472 -{
1473 1476
1474 - std::cout <<"----------------------target--------------------------"<<std::endl;
1475 - std::cout << "lat : "<<mis->lat <<std:: endl;
1476 - std::cout << "lon : "<<mis->lon <<std:: endl;
1477 - std::cout << "alt : "<<mis->altitude <<std:: endl;
1478 - //std::cout << "[whatever] "<<mis->timestamp <<std:: endl;
1479 - mission_index++;
1480 - return 0;
1481 -}
1482 bool 1477 bool
1483 Mission::read_mission_item(int offset, struct mission_item_s *mission_item) 1478 Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
1484 { 1479 {
1485 /* select mission */ 1480 /* select mission */
1486 const int current_index = _current_mission_index; 1481 const int current_index = _current_mission_index;
1487 int index_to_read = current_index + offset; 1482 int index_to_read = current_index + offset;
1483 +
1488 int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read; 1484 int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read;
1489 const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; 1485 const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
1490 1486
...@@ -1564,6 +1560,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) ...@@ -1564,6 +1560,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
1564 return true; 1560 return true;
1565 } 1561 }
1566 } 1562 }
1563 +
1567 /* we have given up, we don't want to cycle forever */ 1564 /* we have given up, we don't want to cycle forever */
1568 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up."); 1565 mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.");
1569 return false; 1566 return false;
...@@ -1865,5 +1862,6 @@ void Mission::publish_navigator_mission_item() ...@@ -1865,5 +1862,6 @@ void Mission::publish_navigator_mission_item()
1865 navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; 1862 navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
1866 1863
1867 navigator_mission_item.timestamp = hrt_absolute_time(); 1864 navigator_mission_item.timestamp = hrt_absolute_time();
1865 +
1868 _navigator_mission_item_pub.publish(navigator_mission_item); 1866 _navigator_mission_item_pub.publish(navigator_mission_item);
1869 } 1867 }
......
...@@ -49,7 +49,7 @@ ...@@ -49,7 +49,7 @@
49 #include "navigator_mode.h" 49 #include "navigator_mode.h"
50 50
51 #include <float.h> 51 #include <float.h>
52 -#include <iostream> 52 +
53 #include <dataman/dataman.h> 53 #include <dataman/dataman.h>
54 #include <drivers/drv_hrt.h> 54 #include <drivers/drv_hrt.h>
55 #include <px4_platform_common/module_params.h> 55 #include <px4_platform_common/module_params.h>
...@@ -227,7 +227,7 @@ private: ...@@ -227,7 +227,7 @@ private:
227 * Find and store the index of the landing sequence (DO_LAND_START) 227 * Find and store the index of the landing sequence (DO_LAND_START)
228 */ 228 */
229 bool find_mission_land_start(); 229 bool find_mission_land_start();
230 - bool print_next_mission(struct mission_item_s *mis); 230 +
231 /** 231 /**
232 * Return the index of the closest mission item to the current global position. 232 * Return the index of the closest mission item to the current global position.
233 */ 233 */
...@@ -269,7 +269,7 @@ private: ...@@ -269,7 +269,7 @@ private:
269 MISSION_TYPE_NONE, 269 MISSION_TYPE_NONE,
270 MISSION_TYPE_MISSION 270 MISSION_TYPE_MISSION
271 } _mission_type{MISSION_TYPE_NONE}; 271 } _mission_type{MISSION_TYPE_NONE};
272 - int mission_index=-1; 272 +
273 bool _inited{false}; 273 bool _inited{false};
274 bool _home_inited{false}; 274 bool _home_inited{false};
275 bool _need_mission_reset{false}; 275 bool _need_mission_reset{false};
......