[modules] digital_cam: add heading and hmsl in DC_SHOT message

also fix DC_SHOT in hackhd
paparazzi · Dec 3, 2014 · deb22d4 · deb22d4
1 parent 8b6383a
commit deb22d4
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 20 deletions.
diff --git a/conf/messages.xml b/conf/messages.xml
@@ -530,9 +530,9 @@
     <field name="mode" type="int16" unit=""/>
     <field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
     <field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
-    <field name="alt" type="int32" unit="mm" alt_unit="m"/>
+    <field name="alt" type="int32" unit="mm" alt_unit="m">altitude above WGS84 reference ellipsoid</field>
     <field name="course" type="float" unit="deg"/>
-    <field name="buffer" type="uint16"/>
+    <field name="photo_nr" type="uint16"/>
     <field name="dist" type="float" unit="m"/>
     <field name="next_dist" type="float" unit="m"/>
     <field name="start_x" type="float" unit="m"/>
@@ -882,14 +882,16 @@
 
   <message name="DC_SHOT" id="110">
     <field name="photo_nr" type="int16"/>
-    <field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
-    <field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
-    <field name="alt" type="int32" unit="mm" alt_unit="m"/>
-    <field name="phi" type="int16" unit="decideg"/>
-    <field name="theta" type="int16" unit="decideg"/>
-    <field name="course" type="int16" unit="decideg"/>
-    <field name="speed"  type="uint16" unit="cm/s"/>
-    <field name="itow"  type="uint32" unit="ms"/>
+    <field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001">Gedetic latitude</field>
+    <field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001">Longitude</field>
+    <field name="alt" type="int32" unit="mm" alt_unit="m">altitude above WGS84 reference ellipsoid</field>
+    <field name="hmsl" type="int32" unit="mm" alt_unit="m">Height above Mean Sea Level (geoid)</field>
+    <field name="phi" type="int16" unit="decideg">Euler angle around x-axis (roll)</field>
+    <field name="theta" type="int16" unit="decideg">Euler angle around y-axis (pitch)</field>
+    <field name="psi" type="int16" unit="decideg">Euler angle around z-axis (yaw)</field>
+    <field name="course" type="int16" unit="decideg">Course over ground (CW/north)</field>
+    <field name="speed" type="uint16" unit="cm/s">horizontal ground speed</field>
+    <field name="itow" type="uint32" unit="ms">GPS time of week</field>
   </message>
 
   <message name="TEST_BOARD_RESULTS" id="111">

diff --git 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
@@ -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