Showing
3 changed files
with
56 additions
and
34 deletions
... | @@ -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}; | ... | ... |
-
Please register or login to post a comment