Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
C
c-astral-wrapper
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
nexedi
c-astral-wrapper
Commits
11a6f663
Commit
11a6f663
authored
Sep 03, 2023
by
Léo-Paul Géneau
👾
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Subcribe to GLOBAL_POSITION_INT messages
Fetch position from GLOBAL_POSITION_INT to get timestamped positions
parent
ce05799a
Changes
1
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
28 additions
and
24 deletions
+28
-24
mavsdk_wrapper.cpp
mavsdk_wrapper.cpp
+28
-24
No files found.
mavsdk_wrapper.cpp
View file @
11a6f663
...
...
@@ -44,6 +44,8 @@ static const double SIN_ADDED_DISTANCE = sin(ADDED_DISTANCE);
static
int
mavsdk_started
=
0
;
static
Telemetry
::
Position
origin
;
static
int64_t
current_position
[
POSITION_ARRAY_SIZE
]
=
{
0
};
static
int64_t
timestamp_difference
=
0
;
static
Coordinates
targeted_destination
;
static
Coordinates
projected_destination
;
...
...
@@ -165,14 +167,12 @@ static void doReposition_async(float la, float lo, float radius, float y) {
std
::
thread
(
doReposition
,
la
,
lo
,
radius
,
y
).
detach
();
}
static
void
updateProjection
(
double
current_lat
,
double
current_lon
)
{
static
void
updateProjection
(
int64_t
current_lat
,
int64_t
current_lon
)
{
projected_destination
=
project
(
targeted_destination
,
bearing
(
current_lat
,
current_lon
,
bearing
(
(
double
)
current_lat
/
1e7
,
(
double
)
current_lon
/
1e7
,
targeted_destination
.
latitude
,
targeted_destination
.
longitude
)
);
printf
(
"distance between follower and projection in C %f
\n
"
,
distance
(
current_lat
,
current_lon
,
targeted_destination
.
latitude
,
targeted_destination
.
longitude
));
// printf("bearing %f\n", toDeg(bearing(targeted_destination.latitude, targeted_destination.longitude, projected_destination.latitude, projected_destination.longitude)));
doReposition_async
(
(
float
)
projected_destination
.
latitude
,
...
...
@@ -189,22 +189,19 @@ static long long int getTimestamp() {
void
updateLogAndProjection
(
void
)
{
std
::
ostringstream
oss
;
Telemetry
::
Position
position
;
Telemetry
::
FixedwingMetrics
metrics
;
long
long
int
timestamp
=
getTimestamp
();
if
(
mavsdk_started
)
{
position
=
telemetry
->
position
();
metrics
=
telemetry
->
fixedwing_metrics
();
oss
<<
timestamp
<<
";"
<<
position
.
latitude_deg
<<
";"
<<
position
.
longitude_deg
<<
";"
<<
position
.
absolute_altitude_m
<<
";"
<<
position
.
relative_altitude_m
<<
";"
oss
<<
current_position
[
4
]
<<
";"
<<
current_position
[
0
]
<<
";"
<<
current_position
[
1
]
<<
";"
<<
current_position
[
2
]
<<
";"
<<
current_position
[
3
]
<<
";"
<<
telemetry
->
attitude_euler
().
yaw_deg
<<
";"
<<
metrics
.
airspeed_m_s
<<
";"
<<
metrics
.
climb_rate_m_s
;
log
(
oss
.
str
());
if
(
projected_destination
.
latitude
!=
0
||
targeted_destination
.
latitude
!=
0
)
{
updateProjection
(
position
.
latitude_deg
,
position
.
longitude_deg
);
updateProjection
(
current_position
[
0
],
current_position
[
1
]
);
}
}
}
...
...
@@ -248,6 +245,20 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
action
=
new
Action
(
msystem
);
mavlink_passthrough
=
new
MavlinkPassthrough
(
msystem
);
mavlink_passthrough
->
subscribe_message_async
(
MAVLINK_MSG_ID_GLOBAL_POSITION_INT
,
[](
const
mavlink_message_t
&
message
)
{
mavlink_global_position_int_t
position
;
mavlink_msg_global_position_int_decode
(
&
message
,
&
position
);
if
(
timestamp_difference
==
0
)
timestamp_difference
=
getTimestamp
()
-
position
.
time_boot_ms
;
current_position
[
0
]
=
position
.
lat
;
current_position
[
1
]
=
position
.
lon
;
current_position
[
2
]
=
position
.
alt
;
current_position
[
3
]
=
position
.
relative_alt
;
current_position
[
4
]
=
position
.
time_boot_ms
+
timestamp_difference
;
});
do
{
log
(
"Waiting for first telemetry"
);
sleep_for
(
seconds
(
1
));
...
...
@@ -260,7 +271,7 @@ int start(const char * ip, int port, const char * log_file, int timeout) {
log
(
"MAVSDK started..."
);
mavsdk_started
=
1
;
log
(
"timestamp (ms);latitude (°
);longitude (°);AMSL (m);rel altitude (
m);yaw (°);air speed (m/s);climb rate (m/s)"
);
log
(
"timestamp (ms);latitude (°
E7);longitude (°E7);AMSL (mm);rel altitude (m
m);yaw (°);air speed (m/s);climb rate (m/s)"
);
return
0
;
}
...
...
@@ -382,18 +393,17 @@ void setTargetCoordinates(double la, double lo, float a) {
return
;
}
Telemetry
::
Position
position
=
telemetry
->
position
();
targeted_destination
.
latitude
=
la
;
targeted_destination
.
longitude
=
lo
;
updateProjection
(
position
.
latitude_deg
,
position
.
longitude_deg
);
updateProjection
(
current_position
[
0
],
current_position
[
1
]
);
setAltitude_async
(
a
);
}
// Information functions
float
getAltitude
(
void
)
{
return
telemetry
->
position
().
absolute_altitude_m
;
return
(
float
)(
current_position
[
2
]
/
1000
)
;
}
float
getInitialAltitude
(
void
)
{
...
...
@@ -408,15 +418,9 @@ double getInitialLongitude(void) {
return
origin
.
longitude_deg
;
}
double
*
getPositionArray
(
void
)
{
double
timestamp
=
(
double
)
getTimestamp
();
Telemetry
::
Position
position
=
telemetry
->
position
();
double
*
positionArray
=
(
double
*
)
malloc
(
POSITION_ARRAY_SIZE
*
sizeof
(
double
));
positionArray
[
0
]
=
position
.
latitude_deg
;
positionArray
[
1
]
=
position
.
longitude_deg
;
positionArray
[
2
]
=
(
double
)
position
.
absolute_altitude_m
;
positionArray
[
3
]
=
(
double
)
position
.
relative_altitude_m
;
positionArray
[
4
]
=
timestamp
;
int64_t
*
getPositionArray
(
void
)
{
int64_t
*
positionArray
=
(
int64_t
*
)
malloc
(
POSITION_ARRAY_SIZE
*
sizeof
(
int64_t
));
memcpy
(
positionArray
,
current_position
,
POSITION_ARRAY_SIZE
*
sizeof
(
int64_t
));
return
positionArray
;
}
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment