gps, battery

conf/airframes/tl.xml

@@ -55,6 +55,9 @@ ap.srcs = tl_main.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
 ap.CFLAGS += -DLED  -DLED -DTIME_LED=1
 
+ap.CFLAGS += -DADC
+ap.srcs += $(SRC_ARCH)/adc_hw.c tl_bat.c
+
 ap.srcs += commands.c
 
 ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT
@@ -69,7 +72,7 @@ ap.srcs += downlink.c $(SRC_ARCH)/uart_hw.c pprz_transport.c tl_telemetry.c tl_d
 
 ap.srcs += tl_autopilot.c tl_control.c tl_estimator.c
 
-ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE
+ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DDOWNLINK_GPS_DEVICE=DOWNLINK_AP_DEVICE -DGPS_CONFIGURE -DUSER_GPS_CONFIGURE=\"tl_gps_configure.h\" -DGPS_BAUD=38400
 ap.srcs += gps_ubx.c gps.c latlong.c
 
 

sw/airborne/tl_bat.c

@@ -0,0 +1,17 @@
+#include "tl_bat.h"
+#include CONFIG
+#include "adc.h"
+#include "airframe.h"
+
+uint8_t tl_bat_decivolt;
+
+struct adc_buf vsupply_adc_buf;
+
+void tl_bat_init( void ) {
+  adc_buf_channel(ADC_CHANNEL_VSUPPLY, &vsupply_adc_buf, DEFAULT_AV_NB_SAMPLE);
+}
+
+
+void tl_bat_periodic_task( void ) {
+  tl_bat_decivolt = VoltageOfAdc((10*(vsupply_adc_buf.sum/vsupply_adc_buf.av_nb_sample)));
+}

sw/airborne/tl_bat.h

@@ -0,0 +1,11 @@
+#ifndef TL_BAT_H
+#define TL_BAT_H
+
+#include "std.h"
+
+extern uint8_t tl_bat_decivolt;
+
+void tl_bat_init( void );
+void tl_bat_periodic_task( void );
+
+#endif // TL_BAT_H

sw/airborne/tl_estimator.c

@@ -3,10 +3,11 @@
 #include "flight_plan.h"
 
 bool_t estimator_in_flight;
+uint16_t estimator_flight_time;
 
 float estimator_east; /* m */
 float estimator_north; /* m */
-float estimator_z; /* m */
+float estimator_z; /* altitude in m */
 
 float estimator_speed; /* m/s */
 float estimator_climb; /* m/s */

sw/airborne/tl_estimator.h

@@ -4,6 +4,16 @@
 #include "std.h"
 
 extern bool_t estimator_in_flight;
+extern uint16_t estimator_flight_time;
+
+extern float estimator_east; /* m */
+extern float estimator_north; /* m */
+extern float estimator_z; /* altitude in m */
+
+extern float estimator_speed; /* m/s */
+extern float estimator_climb; /* m/s */
+extern float estimator_course; /* rad, CCW */
+
 
 void tl_estimator_init(void);
 void tl_estimator_use_gps(void);

sw/airborne/tl_gps_configure.h

@@ -0,0 +1,26 @@
+static bool_t user_gps_configure(bool_t cpt) {
+  switch (cpt) {
+  case 0:
+    UbxSend_CFG_NAV(NAV_DYN_PEDESTRIAN, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
+    break;
+  case 1:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
+    break;
+  case 2:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
+    break;
+  case 3:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_STATUS_ID, 0, 1, 0, 0);
+    break;
+  case 4:
+    UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 0, 4, 0, 0);
+    break;
+  case 5:
+    UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00);
+    break;
+  case 6:
+    UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000);   
+    return FALSE;
+  }
+  return TRUE; /* Continue, except for the last case */
+}

sw/airborne/tl_main.c

@@ -15,6 +15,8 @@
 #include "datalink.h"
 #include "tl_autopilot.h"
 #include "tl_estimator.h"
+#include "adc.h"
+#include "tl_bat.h"
 #include "gps.h"
 
 static inline void tl_main_init( void );
@@ -36,6 +38,8 @@ int main( void ) {
 static inline void tl_main_init( void ) {
   hw_init();
   led_init();
+  adc_init();
+  tl_bat_init();
   sys_time_init();
 
   actuators_init();
@@ -55,50 +59,25 @@ static inline void tl_main_init( void ) {
 }
 
 
-static inline void tl_main_periodic_task( void ) {
-  
-  /* run control loops */
-  tl_autopilot_periodic_task();
-
-  SetActuatorsFromCommands(commands);
+static inline void tl_main_periodic_task( void ) {  
+  tl_bat_periodic_task();
 
   radio_control_periodic_task();
   if (rc_status != RC_OK)
      tl_autopilot_mode = TL_AP_MODE_FAILSAFE;
   
-  tl_telemetry_periodic_task();
-}
+  /* run control loops */
+  tl_autopilot_periodic_task();
 
+  tl_telemetry_periodic_task();
+
+  SetActuatorsFromCommands(commands);
+}
 
 
 static inline void tl_main_event_task( void ) {
-  
-  //DlEventCheckAndHandle();
-
   RadioControlEventCheckAndHandle(tl_autopilot_on_rc_event);
 
-#ifdef GPS
-  if (GpsBuffer()) {
-    ReadGpsBuffer();
-  }
-  if (gps_msg_received) {
-    /* parse and use GPS messages */
-#ifdef GPS_CONFIGURE
-    if (gps_status_config < GPS_CONFIG_DONE)		
-      gps_configure();
-    else
-#endif
-      parse_gps_msg();
-    gps_msg_received = FALSE;
-    if (gps_pos_available) {
-      gps_verbose_downlink = !estimator_in_flight;
-      UseGpsPos(tl_estimator_use_gps);
-      gps_pos_available = FALSE;
-    }
-  }
-#endif /** GPS */
-
-#if defined DATALINK 
+  GpsEventCheckAndHandle(tl_estimator_use_gps, !estimator_in_flight);
   DlEventCheckAndHandle();
-#endif // DATALINK
 }

sw/airborne/tl_telemetry.h

@@ -8,6 +8,10 @@
 #include "radio_control.h"
 #include "commands.h"
 #include "actuators.h"
+#include "tl_bat.h"
+#include "tl_estimator.h"
+
+
 
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 
@@ -15,6 +19,8 @@
 #define PERIODIC_SEND_PPM() DOWNLINK_SEND_PPM(PPM_NB_PULSES, ppm_pulses)
 #define PERIODIC_SEND_COMMANDS() DOWNLINK_SEND_COMMANDS(COMMANDS_NB, commands)
 #define PERIODIC_SEND_ACTUATORS() DOWNLINK_SEND_ACTUATORS(SERVOS_NB, actuators)
+#define PERIODIC_SEND_BAT() { int16_t dummy16=42; uint8_t dummy8=42; DOWNLINK_SEND_BAT(&commands[COMMAND_THROTTLE], &tl_bat_decivolt, &estimator_flight_time, &dummy8, &dummy16, &dummy16, &dummy16); }
+#define PERIODIC_SEND_ESTIMATOR() DOWNLINK_SEND_ESTIMATOR(&estimator_z, &estimator_climb)