Toggle navigation
Toggle navigation
This project
Loading...
Sign in
2021-1-capstone-design1
/
JJS_Project2
Go to a project
Toggle navigation
Toggle navigation pinning
Projects
Groups
Snippets
Help
Project
Activity
Repository
Pipelines
Graphs
Issues
0
Merge Requests
0
Wiki
Snippets
Network
Create a new issue
Builds
Commits
Issue Boards
Authored by
이충섭
2021-06-11 17:14:21 +0900
Browse Files
Options
Browse Files
Download
Email Patches
Plain Diff
Commit
7065b9da6e0edcb8cb90bfa4a063abcc3cd4405a
7065b9da
1 parent
67e0e540
fix : print target gps fucntion by using dataman in ekf2
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
56 additions
and
34 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/navigator/mission.cpp
home/Firmware/src/modules/navigator/mission.h
home/Firmware/src/modules/ekf2/EKF2.cpp
View file @
7065b9d
...
...
@@ -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
);
...
...
home/Firmware/src/modules/navigator/mission.cpp
View file @
7065b9d
...
...
@@ -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
);
}
...
...
home/Firmware/src/modules/navigator/mission.h
View file @
7065b9d
...
...
@@ -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
};
...
...
Please
register
or
login
to post a comment