diff --git a/conf/messages.xml b/conf/messages.xml
index 40945a9b11..a23c1a794d 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -291,6 +291,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -412,13 +435,16 @@
+
-
-
-
-
+
+
+
+
+
+
diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h
index 28d0acd9b5..97d30fcad7 100644
--- a/sw/airborne/ap_downlink.h
+++ b/sw/airborne/ap_downlink.h
@@ -176,7 +176,7 @@
#define PERIODIC_SEND_AIRSPEED(_chan) {}
#endif
-#define PERIODIC_SEND_ENERGY(_chan) Downlink({ int16_t e = energy; DOWNLINK_SEND_ENERGY( &vsup, &curs, &e); })
+#define PERIODIC_SEND_ENERGY(_chan) Downlink({ int16_t e = energy; float vsup = ((float)vsupply) / 10.0f; float curs = ((float) current)/1000.0f; DOWNLINK_SEND_ENERGY(_chan, &vsup, &curs, &e); })
#include "fw_h_ctl_a.h"
diff --git a/sw/airborne/dc.c b/sw/airborne/dc.c
index 839265e96c..97987b9118 100644
--- a/sw/airborne/dc.c
+++ b/sw/airborne/dc.c
@@ -4,3 +4,6 @@ uint8_t dc_timer;
uint8_t dc_periodic_shutter;
uint8_t dc_shutter_timer;
uint8_t dc_utm_threshold;
+uint16_t dc_photo_nr = 0;
+
+uint8_t dc_shoot = 0;
diff --git a/sw/airborne/dc.h b/sw/airborne/dc.h
index 835c2cf326..1017c57c9a 100644
--- a/sw/airborne/dc.h
+++ b/sw/airborne/dc.h
@@ -67,24 +67,38 @@ extern uint8_t dc_shutter_timer;
extern uint8_t dc_utm_threshold;
/* In m. If non zero, automatic shots when greater than utm_north % 100 */
+/* Picture Number starting from zero */
+extern uint16_t dc_photo_nr;
+extern uint8_t dc_shoot;
+#ifndef DC_PUSH
+#define DC_PUSH LED_ON
+#endif
+
+#ifndef DC_RELEASE
+#define DC_RELEASE LED_OFF
+#endif
#define SHUTTER_DELAY 2 /* 4Hz -> 0.5s */
static inline uint8_t dc_shutter( void ) {
dc_timer = SHUTTER_DELAY;
- LED_OFF(DC_SHUTTER_LED);
+ DC_PUSH(DC_SHUTTER_LED);
- int16_t phi = DegOfRad(estimator_phi);
- int16_t theta = DegOfRad(estimator_theta);
- DOWNLINK_SEND_DC_SHOT(DefaultChannel, &gps_utm_east, &gps_utm_north, &gps_utm_zone, &gps_course, &estimator_z, &phi, &theta);
+ int16_t phi = DegOfRad(estimator_phi*10.0f);
+ int16_t theta = DegOfRad(estimator_theta*10.0f);
+ DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &estimator_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow);
+
+ dc_photo_nr++;
return 0;
}
static inline uint8_t dc_zoom( void ) {
dc_timer = SHUTTER_DELAY;
- LED_OFF(DC_ZOOM_LED);
+#ifdef DC_ZOOM_LED
+ DC_PUSH(DC_ZOOM_LED);
+#endif
return 0;
}
@@ -95,13 +109,41 @@ static inline uint8_t dc_zoom( void ) {
#define dc_init() { /* initialized as leds */ dc_periodic_shutter = 0; } /* Output */
+static inline void dc_shoot_on_gps( void ) {
+ static uint8_t gps_msg_counter = 0;
+
+ if (dc_shoot > 0)
+ {
+
+ if (gps_msg_counter == 0)
+ {
+ DC_PUSH(DC_SHUTTER_LED);
+ int16_t phi = DegOfRad(estimator_phi*10.0f);
+ int16_t theta = DegOfRad(estimator_theta*10.0f);
+ float gps_z = ((float)gps_alt) / 100.0f;
+ DOWNLINK_SEND_DC_SHOT(DefaultChannel, &dc_photo_nr, &gps_utm_east, &gps_utm_north, &gps_z, &gps_utm_zone, &phi, &theta, &gps_course, &gps_gspeed, &gps_itow);
+ dc_photo_nr++;
+ }
+ else if (gps_msg_counter == 1)
+ {
+ DC_RELEASE(DC_SHUTTER_LED);
+ }
+
+ gps_msg_counter++;
+ if (gps_msg_counter >= 4)
+ gps_msg_counter = 0;
+ }
+}
+
/* 4Hz */
static inline void dc_periodic( void ) {
if (dc_timer) {
dc_timer--;
} else {
- LED_ON(DC_SHUTTER_LED);
- LED_ON(DC_ZOOM_LED);
+ DC_RELEASE(DC_SHUTTER_LED);
+#ifdef DC_ZOOM_LED
+ DC_RELEASE(DC_ZOOM_LED);
+#endif
}
if (dc_periodic_shutter) {