diff --git a/conf/messages.xml b/conf/messages.xml
index e324b276f4e..77c1b40a5d6 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -530,9 +530,9 @@
-
+ altitude above WGS84 reference ellipsoid
-
+
@@ -882,14 +882,16 @@
-
-
-
-
-
-
-
-
+ Gedetic latitude
+ Longitude
+ altitude above WGS84 reference ellipsoid
+ Height above Mean Sea Level (geoid)
+ Euler angle around x-axis (roll)
+ Euler angle around y-axis (pitch)
+ Euler angle around z-axis (yaw)
+ Course over ground (CW/north)
+ horizontal ground speed
+ GPS time of week
diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c
index d9890beef4c..e91130e78a2 100644
--- a/sw/airborne/modules/digital_cam/dc.c
+++ b/sw/airborne/modules/digital_cam/dc.c
@@ -94,8 +94,10 @@ uint16_t dc_photo_nr = 0;
void dc_send_shot_position(void)
{
+ // angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
+ int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
@@ -112,8 +114,10 @@ void dc_send_shot_position(void)
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
+ &gps.hmsl,
&phi,
&theta,
+ &psi,
&course,
&speed,
&gps.tow);
diff --git a/sw/airborne/modules/digital_cam/hackhd.c b/sw/airborne/modules/digital_cam/hackhd.c
index 7eb4abe5693..2754e89f44c 100644
--- a/sw/airborne/modules/digital_cam/hackhd.c
+++ b/sw/airborne/modules/digital_cam/hackhd.c
@@ -90,20 +90,27 @@ static inline uint16_t pin_of_gpio(uint32_t __attribute__((unused)) port, uint16
static inline void hackhd_send_shot_position(void)
{
+ // angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
- int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
+ int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
+ // course in decideg
+ int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
+ // ground speed in cm/s
+ uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
- &hackhd.photo_nr,
- &stateGetPositionLla_i()->lat,
- &stateGetPositionLla_i()->lon,
- &stateGetPositionLla_i()->alt,
- &phi,
- &theta,
- &course,
- &gps.gspeed,
- &gps.tow);
+ &hackhd.photo_nr,
+ &stateGetPositionLla_i()->lat,
+ &stateGetPositionLla_i()->lon,
+ &stateGetPositionLla_i()->alt,
+ &gps.hmsl,
+ &phi,
+ &theta,
+ &psi,
+ &course,
+ &speed,
+ &gps.tow);
}
#endif