[modules] fix DC_SHOT in hackhd

paparazzi · Dec 3, 2014 · c3efd60 · c3efd60
1 parent f54a456
commit c3efd60
Showing 1 changed file with 17 additions and 10 deletions.
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