[paparazzi-commits] [4091] AirSpeed

[Christophe De Wagter]

Revision: 4091
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4091
Author:   dewagter
Date:     2009-09-07 14:54:03 +0000 (Mon, 07 Sep 2009)
Log Message:
-----------
AirSpeed

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/holiday50.xml
    paparazzi3/trunk/conf/autopilot/tiny.makefile
    paparazzi3/trunk/conf/messages.xml
    paparazzi3/trunk/conf/settings/light.xml
    paparazzi3/trunk/sw/airborne/ap_downlink.h
    paparazzi3/trunk/sw/airborne/datalink.c
    paparazzi3/trunk/sw/airborne/estimator.c
    paparazzi3/trunk/sw/airborne/estimator.h
    paparazzi3/trunk/sw/airborne/fw_v_ctl.c
    paparazzi3/trunk/sw/airborne/fw_v_ctl.h
    paparazzi3/trunk/sw/airborne/light.h
    paparazzi3/trunk/sw/airborne/main_ap.c

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/airspeed.c
    paparazzi3/trunk/sw/airborne/airspeed.h

Property Changed:
----------------
    paparazzi3/trunk/sw/ground_segment/multimon/

Modified: paparazzi3/trunk/conf/airframes/holiday50.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/holiday50.xml       2009-09-07 10:13:15 UTC 
(rev 4090)
+++ paparazzi3/trunk/conf/airframes/holiday50.xml       2009-09-07 14:54:03 UTC 
(rev 4091)
@@ -25,7 +25,7 @@
   </rc_commands>
 
   <ap_only_commands>
-    <copy command="CAM_TILT" />
+    <copy command="CAM_TILT"/>
   </ap_only_commands>
 
   <command_laws>
@@ -148,7 +148,7 @@
                float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * 
v_ctl_auto_throttle_pitch_of_vz_dgain) 
                * v_ctl_auto_throttle_pitch_of_vz_pgain;
        -->
-    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.368"/>
+    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" 
value="0.319999992847"/>
     <define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.20"/>
     <define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.45"/>
     <define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
@@ -173,7 +173,7 @@
     <define name="PITCH_MAX_SETPOINT" value="0.4" unit="radians"/>
     <define name="PITCH_MIN_SETPOINT" value="-0.4" unit="radians"/>
 
-    <define name="PITCH_PGAIN" value="-8000."/>        <!-- Pitch Angle PD 
control -->
+    <define name="PITCH_PGAIN" value="-4925.65087891"/>        <!-- Pitch 
Angle PD control -->
     <define name="PITCH_DGAIN" value="2.0"/>
 
     <define name="ELEVATOR_OF_ROLL" value="2050"/> <!-- Feed forward ABS(roll) 
to elevator -->
@@ -298,8 +298,8 @@
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny_1_1.h\" -DAGR_CLIMB -DLOITER_TRIM 
-DALT_KALMAN -DWIND_INFO -DSTRONG_WIND
 sim.srcs += nav_survey_rectangle.c nav_line.c 
 # traffic_info.c
-#sim.CFLAGS += -DUSE_AIRSPEED
-#sim.srcs += airspeed.c
+sim.CFLAGS += -DUSE_AIRSPEED
+sim.srcs += airspeed.c
 
 sim.CFLAGS += -DCAM -DMOBILE_CAM -DPOINT_CAM -DPOINT_CAM_PITCH_ROLL
 # -DTEST_CAM

Modified: paparazzi3/trunk/conf/autopilot/tiny.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/tiny.makefile       2009-09-07 10:13:15 UTC 
(rev 4090)
+++ paparazzi3/trunk/conf/autopilot/tiny.makefile       2009-09-07 14:54:03 UTC 
(rev 4091)
@@ -7,6 +7,11 @@
 ap.TARGET = autopilot
 ap.TARGETDIR = autopilot
 
+fbw.ARCHDIR = $(ARCHI)
+fbw.ARCH = arm7tdmi
+fbw.TARGET = autopilot
+fbw.TARGETDIR = autopilot
+
 LPC21ISP_BAUD = 38400
 LPC21ISP_XTAL = 12000
 

Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml  2009-09-07 10:13:15 UTC (rev 4090)
+++ paparazzi3/trunk/conf/messages.xml  2009-09-07 14:54:03 UTC (rev 4091)
@@ -222,6 +222,11 @@
    <field name="long" type="float" unit="deg"/>
  </message>
 
+ <message name="MOTOR" id="34">
+   <field name="rpm" type="uint16" unit="Hz"/>
+   <field name="current" type="uint16" unit="mA"/>
+ </message>
+
  <message name="WP_MOVED" id="35">
     <field name="wp_id" type="uint8"/>
     <field name="utm_east" type="float" unit="m"></field>
@@ -301,6 +306,12 @@
     <field name="time" type="uint32" unit="us"></field>
  </message>
 
+  <message name="ENERGY" id="37">
+    <field name="bat"      type="float" unit="V"/>
+    <field name="amp"      type="float" unit="A"/>
+    <field name="energy"   type="uint16" unit="Wh"/>
+  </message>
+
  <message name="WINDTURBINE_STATUS_" id="50">
    <field name="ac_id" type="uint8"/>
    <field name="tb_id" type="uint8"/>

Modified: paparazzi3/trunk/conf/settings/light.xml
===================================================================
--- paparazzi3/trunk/conf/settings/light.xml    2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/conf/settings/light.xml    2009-09-07 14:54:03 UTC (rev 
4091)
@@ -3,7 +3,10 @@
 <settings>
     <dl_settings NAME="lights control">
        <dl_settings NAME="lights">
-          <dl_setting module="light" var="light_duration_1" min="0" max="61" 
step="1"/>
+          <dl_setting module="light" var="light_duration_1" min="0" max="61" 
step="1">
+            <strip_button name="CamOn" icon="on.png" value="15"/>
+            <strip_button name="CamOff" icon="off.png" value="0"/>
+         </dl_setting>
           <dl_setting module="light" var="light_duration_2" min="0" max="61" 
step="1"/>
        </dl_settings>
     </dl_settings>

Added: paparazzi3/trunk/sw/airborne/airspeed.c
===================================================================
--- paparazzi3/trunk/sw/airborne/airspeed.c                             (rev 0)
+++ paparazzi3/trunk/sw/airborne/airspeed.c     2009-09-07 14:54:03 UTC (rev 
4091)
@@ -0,0 +1,41 @@
+#include "airspeed.h"
+#include "adc.h"
+#include "airframe.h"
+#include "estimator.h"
+
+#ifdef USE_AIRSPEED
+uint16_t adc_airspeed_val;
+#else
+#error "You compiled the airspeed.c file but did not USE_AIRSPEED, which is 
needed in other *.c files"
+#endif
+
+#ifdef ADC_CHANNEL_AIRSPEED
+#ifndef SITL
+static struct adc_buf buf_airspeed;
+#endif
+#else
+#error "You compiled the airspeed.c file but did not ADC_CHANNEL_AIRSPEED, 
which is needed in other *.c files"
+#endif
+
+void airspeed_init( void ) {
+#ifdef ADC_CHANNEL_AIRSPEED
+#  ifndef ADC_CHANNEL_AIRSPEED_NB_SAMPLES
+#    error "You defined USE_AIRSPEED but did not assign a 
ADC_CHANNEL_AIRSPEED_NB_SAMPLES"
+#  endif
+#  ifndef SITL
+     adc_buf_channel(ADC_CHANNEL_AIRSPEED, &buf_airspeed, 
ADC_CHANNEL_AIRSPEED_NB_SAMPLES);
+#  endif
+#else
+#  error "You defined USE_AIRSPEED but did not assign a ADC_CHANNEL_AIRSPEED"
+#endif
+}
+
+void airspeed_update( void ) {
+#ifdef ADC_CHANNEL_AIRSPEED
+#ifndef SITL
+  adc_airspeed_val = (buf_airspeed.sum / buf_airspeed.av_nb_sample) - 
AIRSPEED_ZERO;
+  float airspeed = AIRSPEED_SCALE * adc_airspeed_val;
+  EstimatorSetAirspeed(airspeed);
+#endif
+#endif
+}

Added: paparazzi3/trunk/sw/airborne/airspeed.h
===================================================================
--- paparazzi3/trunk/sw/airborne/airspeed.h                             (rev 0)
+++ paparazzi3/trunk/sw/airborne/airspeed.h     2009-09-07 14:54:03 UTC (rev 
4091)
@@ -0,0 +1,13 @@
+#ifndef AIRSPEED_H
+#define AIRSPEED_H
+
+#include <inttypes.h>
+
+#ifdef USE_AIRSPEED
+extern uint16_t adc_airspeed_val;
+#endif
+
+void airspeed_init( void );
+void airspeed_update( void );
+
+#endif /* ADC_GENERIC_H */

Modified: paparazzi3/trunk/sw/airborne/ap_downlink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/ap_downlink.h  2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/ap_downlink.h  2009-09-07 14:54:03 UTC (rev 
4091)
@@ -173,6 +173,15 @@
 #define PERIODIC_SEND_BARO_MS5534A(_chan) {}
 #endif
 
+#ifdef USE_AIRSPEED
+#define PERIODIC_SEND_AIRSPEED(_chan) DOWNLINK_SEND_AIRSPEED (_chan, 
&adc_airspeed_val,&estimator_airspeed)
+#else
+#define PERIODIC_SEND_AIRSPEED(_chan) {}
+#endif
+
+#define PERIODIC_SEND_ENERGY(_chan) Downlink({ int16_t e = energy; 
DOWNLINK_SEND_ENERGY( &vsup, &curs, &e); })
+
+
 #include "fw_h_ctl_a.h"
 #define PERIODIC_SEND_H_CTL_A(_chan) DOWNLINK_SEND_H_CTL_A(_chan, 
&h_ctl_roll_sum_err, &h_ctl_ref_roll_angle, &h_ctl_pitch_sum_err, 
&h_ctl_ref_pitch_angle)
 

Modified: paparazzi3/trunk/sw/airborne/datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/datalink.c     2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/datalink.c     2009-09-07 14:54:03 UTC (rev 
4091)
@@ -114,7 +114,9 @@
     if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) {
     wind_east = DL_WIND_INFO_east(dl_buffer);
     wind_north = DL_WIND_INFO_north(dl_buffer);
+#ifndef USE_AIRSPEED
     estimator_airspeed = DL_WIND_INFO_airspeed(dl_buffer);
+#endif
 #ifdef WIND_INFO_RET
     DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, &wind_east, &wind_north, 
&estimator_airspeed);
 #endif

Modified: paparazzi3/trunk/sw/airborne/estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/estimator.c    2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/estimator.c    2009-09-07 14:54:03 UTC (rev 
4091)
@@ -95,6 +95,10 @@
 
   EstimatorSetRate(0., 0.);
 
+#ifdef USE_AIRSPEED
+  EstimatorSetAirspeed( 0. );
+#endif
+
   estimator_flight_time = 0;
 
   estimator_airspeed = NOMINAL_AIRSPEED;

Modified: paparazzi3/trunk/sw/airborne/estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/estimator.h    2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/estimator.h    2009-09-07 14:54:03 UTC (rev 
4091)
@@ -110,6 +110,9 @@
 
 #endif
 
+#ifdef USE_AIRSPEED
+#define EstimatorSetAirspeed(airspeed) { estimator_airspeed = airspeed; }
+#endif
 
 #define EstimatorSetAtt(phi, psi, theta) { estimator_phi = phi; estimator_psi 
= psi; estimator_theta = theta; }
 #define EstimatorSetPhiPsi(phi, psi) { estimator_phi = phi; estimator_psi = 
psi; }

Modified: paparazzi3/trunk/sw/airborne/fw_v_ctl.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl.c     2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl.c     2009-09-07 14:54:03 UTC (rev 
4091)
@@ -78,6 +78,19 @@
 inline static void v_ctl_climb_auto_pitch_loop( void );
 #endif
 
+#ifdef USE_AIRSPEED
+float v_ctl_auto_airspeed_setpoint;
+float v_ctl_auto_airspeed_pitch_pgain;
+float v_ctl_auto_airspeed_throttle_pgain;
+float v_ctl_auto_airspeed_throttle_igain;
+
+float v_ctl_auto_airspeed_throttle_sum_err;
+#define V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR 100
+
+inline void v_ctl_airspeed_loop( void );
+#endif
+
+
 void v_ctl_init( void ) {
   /* mode */
   v_ctl_mode = V_CTL_MODE_MANUAL;
@@ -114,6 +127,15 @@
   v_ctl_auto_pitch_sum_err = 0.;
 #endif
 
+#ifdef USE_AIRSPEED
+  v_ctl_auto_airspeed_setpoint = V_CTL_AUTO_AIRSPEED_SETPOINT;
+  v_ctl_auto_airspeed_pitch_pgain = V_CTL_AUTO_AIRSPEED_PITCH_PGAIN;
+  v_ctl_auto_airspeed_throttle_pgain = V_CTL_AUTO_AIRSPEED_THROTTLE_PGAIN;
+  v_ctl_auto_airspeed_throttle_igain = V_CTL_AUTO_AIRSPEED_THROTTLE_IGAIN;
+
+  v_ctl_auto_airspeed_throttle_sum_err = 0.;
+#endif
+
   v_ctl_throttle_setpoint = 0;
 }
 
@@ -176,6 +198,18 @@
   /* pitch pre-command */
   float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * 
v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain;
 
+#ifdef USE_AIRSPEED
+  float err_airspeed = (v_ctl_auto_airspeed_setpoint - estimator_airspeed);
+
+  v_ctl_auto_airspeed_throttle_sum_err += err_airspeed;
+  BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, 
V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR);
+
+  float v_ctl_auto_airspeed_pitch_of_airspeed = (err_airspeed) * 
v_ctl_auto_airspeed_pitch_pgain;
+  float v_ctl_auto_airspeed_throttle_of_airspeed = (err_airspeed + 
v_ctl_auto_airspeed_throttle_sum_err * v_ctl_auto_airspeed_throttle_igain) * 
v_ctl_auto_airspeed_throttle_pgain;
+
+  controlled_throttle += v_ctl_auto_airspeed_throttle_of_airspeed;
+#endif
+
 #if defined AGR_CLIMB
   switch (v_ctl_auto_throttle_submode) {
   case V_CTL_AUTO_THROTTLE_AGRESSIVE:
@@ -216,6 +250,11 @@
     break;
   } /* switch submode */
 #endif
+
+#ifdef USE_AIRSPEED
+  nav_pitch += v_ctl_auto_airspeed_pitch_of_airspeed;
+#endif
+
   v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
 }
 

Modified: paparazzi3/trunk/sw/airborne/fw_v_ctl.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fw_v_ctl.h     2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/fw_v_ctl.h     2009-09-07 14:54:03 UTC (rev 
4091)
@@ -87,6 +87,14 @@
 extern void v_ctl_altitude_loop( void );
 extern void v_ctl_climb_loop ( void );
 
+#ifdef USE_AIRSPEED
+/* "airspeed" inner loop parameters */
+extern float v_ctl_auto_airspeed_setpoint;
+extern float v_ctl_auto_airspeed_pitch_pgain;
+extern float v_ctl_auto_airspeed_throttle_pgain;
+extern float v_ctl_auto_airspeed_throttle_igain;
+#endif
+
 /** Computes throttle_slewed from throttle_setpoint */
 extern void v_ctl_throttle_slew( void );
 

Modified: paparazzi3/trunk/sw/airborne/light.h
===================================================================
--- paparazzi3/trunk/sw/airborne/light.h        2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/light.h        2009-09-07 14:54:03 UTC (rev 
4091)
@@ -36,10 +36,13 @@
 #define LightPeriodicTask2(_1Hz) {}
 #endif /* LIGHT_LED_1 */
 
+#ifndef LIGHT_DURATION_INITIAL
+#define LIGHT_DURATION_INITIAL 0
+#endif
 
 #define LightInit() { \
-  light_duration_1 = 0; \
-  light_duration_2 = 0; \
+  light_duration_1 = LIGHT_DURATION_INITIAL; \
+  light_duration_2 = LIGHT_DURATION_INITIAL; \
 }
 
 

Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c      2009-09-07 10:13:15 UTC (rev 
4090)
+++ paparazzi3/trunk/sw/airborne/main_ap.c      2009-09-07 14:54:03 UTC (rev 
4091)
@@ -69,6 +69,10 @@
 #include "adc_generic.h"
 #endif
 
+#ifdef USE_AIRSPEED
+#include "airspeed.h"
+#endif
+
 #ifdef TELEMETER
 #include "srf08.h"
 #endif
@@ -362,7 +366,7 @@
   SEND_NAVIGATION(DefaultChannel);
 #endif
 
-  SEND_CAM();
+  SEND_CAM(DefaultChannel);
   
   /* The nav task computes only nav_altitude. However, we are interested
      by desired_altitude (= nav_alt+alt_shift) in any case.
@@ -580,6 +584,9 @@
 #if defined GYRO
       gyro_update();
 #endif
+#if defined USE_AIRSPEED
+      airspeed_update();
+#endif
 #ifdef INFRARED
       ir_update();
       estimator_update_state_infrared();
@@ -629,6 +636,9 @@
 #ifdef GYRO
   gyro_init();
 #endif
+#ifdef USE_AIRSPEED
+  airspeed_init();
+#endif
 #ifdef GPS
   gps_init();
 #endif


Property changes on: paparazzi3/trunk/sw/ground_segment/multimon
___________________________________________________________________
Modified: svn:ignore
   - *.cmo
*.cma
*.cmi
*.cmx
.depend
costabf.c
mkcostab
multimon
costabi.c

   + *.cmo
*.cma
*.cmi
*.cmx
.depend
costabf.c
mkcostab
*multimon*.*
costabi.c