Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

[modules] mavlink: send position

Browse Source
This commit is contained in:
Felix Ruess
2014-12-08 21:45:09 +01:00
parent 4f2910aa30
commit 0b948edc78
2 changed files with 41 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/mavlink.xml
+1 -1
View File
@@ -11,7 +11,7 @@
</header>
<init fun="mavlink_init()"/>
<periodic fun="mavlink_periodic()" autorun="TRUE"/>
<periodic fun="mavlink_periodic()" freq="10" autorun="TRUE"/>
<event fun="mavlink_event()"/>
<makefile>
sw/airborne/modules/datalink/mavlink.c
+40 -4
View File
@@ -44,6 +44,8 @@ static char mavlink_params[NB_SETTING][16] = SETTINGS; /**< Transmitting paramet
static inline void mavlink_send_heartbeat(void);
static inline void mavlink_send_sys_status(void);
static inline void mavlink_send_attitude(void);
static inline void mavlink_send_local_position_ned(void);
static inline void mavlink_send_global_position_int(void);
static inline void mavlink_send_params(void);
//TODO FIXME
@@ -63,10 +65,12 @@ void mavlink_init(void)
*/
void mavlink_periodic(void)
{
RunOnceEvery(MODULES_FREQUENCY / 2, mavlink_send_heartbeat());
RunOnceEvery(MODULES_FREQUENCY / 10, mavlink_send_sys_status());
RunOnceEvery(MODULES_FREQUENCY / 10, mavlink_send_attitude());
RunOnceEvery(MODULES_FREQUENCY / 10, mavlink_send_params());
RunOnceEvery(2, mavlink_send_heartbeat());
RunOnceEvery(5, mavlink_send_sys_status());
RunOnceEvery(5, mavlink_send_attitude());
RunOnceEvery(5, mavlink_send_params());
RunOnceEvery(5, mavlink_send_local_position_ned());
RunOnceEvery(5, mavlink_send_global_position_int());
}
/**
@@ -220,6 +224,38 @@ static inline void mavlink_send_attitude(void)
MAVLink(SendMessage());
}
static inline void mavlink_send_local_position_ned(void)
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionNed_f()->x,
stateGetPositionNed_f()->y,
stateGetPositionNed_f()->z,
stateGetSpeedNed_f()->x,
stateGetSpeedNed_f()->y,
stateGetSpeedNed_f()->z);
MAVLink(SendMessage());
}
static inline void mavlink_send_global_position_int(void)
{
float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
if (heading < 0.) {
heading += 360;
}
uint16_t compass_heading = heading * 100;
int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt,
relative_alt,
0,0,0, // velocity in lla?
compass_heading);
MAVLink(SendMessage());
}
/**
* Send the parameters
*/