[paparazzi-commits] [6071] Update passthrough

[Allen Ibara]

Revision: 6071
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6071
Author:   aibara
Date:     2010-10-06 04:31:46 +0000 (Wed, 06 Oct 2010)
Log Message:
-----------
Update passthrough

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c

Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c       
2010-10-06 04:27:26 UTC (rev 6070)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_stm_passthrough_main.c       
2010-10-06 04:31:46 UTC (rev 6071)
@@ -30,8 +30,16 @@
 #include "actuators/actuators_pwm.h"
 #include <firmwares/rotorcraft/imu.h>
 #include "booz/booz_radio_control.h"
+#include "autopilot.h"
+#include "ins.h"
+#include "guidance.h"
+#include "navigation.h"
 #include "lisa/lisa_overo_link.h"
 #include "airframe.h"
+#include "ahrs.h"
+#ifdef PASSTHROUGH_CYGNUS
+#include "stabilization.h"
+#endif
 
 #include "stm32/can.h"
 #include "csc_msg_def.h"
@@ -64,27 +72,33 @@
 
 struct CscVaneMsg csc_vane_msg;
 
-static struct adc_buf adc0_buf; 
-static struct adc_buf adc1_buf; 
-static struct adc_buf adc2_buf; 
-static struct adc_buf adc3_buf; 
+static struct adc_buf adc0_buf;
+static struct adc_buf adc1_buf;
+static struct adc_buf adc2_buf;
+static struct adc_buf adc3_buf;
 
 extern uint8_t adc_new_data_trigger;
 
+struct CscServoCmd csc_servo_cmd;
+
 #define ActuatorsCommit() actuators_pwm_commit();
 #define actuators actuators_pwm_values
 
+#define ChopServo(x,a,b) Chop(x, a, b)
+#define Actuator(i) actuators[i]
+#define SERVOS_TICS_OF_USEC(_s) (_s)
+
 int main(void) {
 
-  main_init();
+       main_init();
 
-  while (1) {
-    if (sys_time_periodic())
-      main_periodic();
-    main_event();
-  }
-  
-  return 0;
+       while (1) {
+               if (sys_time_periodic())
+                       main_periodic();
+               main_event();
+       }
+
+       return 0;
 }
 
 static inline void main_init(void) {
@@ -97,20 +111,41 @@
        actuators_init();
        overo_link_init();
        cscp_init();
-       adc_init();     
-       
+       adc_init();
+
+#ifdef PASSTHROUGH_CYGNUS
+       autopilot_init();
+       nav_init();
+       guidance_h_init();
+       guidance_v_init();
+       stabilization_init();
+
+       ahrs_aligner_init();
+       ahrs_init();
+
+       ins_init();
+#endif
+
        adc_buf_channel(0, &adc0_buf, 8);
        adc_buf_channel(1, &adc1_buf, 8);
        adc_buf_channel(2, &adc2_buf, 8);
        adc_buf_channel(3, &adc3_buf, 8);
-       
+
        cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void 
*)&csc_vane_msg);
        new_radio_msg = FALSE;
+       new_baro_diff = FALSE;
+       new_baro_abs = FALSE;
+       new_vane = FALSE;
+       new_adc = FALSE;
 
+       overo_link.up.msg.imu_tick = 0;
 }
 
 static void check_radio_lost(void)
 {
+#ifdef PASSTHROUGH_CYGNUS
+       return;
+#endif
        if (radio_control.status == RADIO_CONTROL_REALLY_LOST) {
                const static int32_t commands_failsafe[COMMANDS_NB] = 
COMMANDS_FAILSAFE;
                SetActuatorsFromCommands(commands_failsafe);
@@ -122,96 +157,116 @@
        uint16_t v2 = 123;
 
   imu_periodic();
-  OveroLinkPeriodic(on_overo_link_lost);
+#ifdef PASSTHROUGH_CYGNUS
+       autopilot_periodic();
+#endif
+       OveroLinkPeriodic(on_overo_link_lost);
 
-  RunOnceEvery(10, {
-      LED_PERIODIC(); 
-      DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
-      radio_control_periodic();
+       RunOnceEvery(10, {
+                       LED_PERIODIC();
+                       DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
+                       radio_control_periodic();
                        check_radio_lost();
-    });
+               });
 
-  RunOnceEvery(2, {baro_periodic();});
-       
-  if (adc_new_data_trigger) { 
-    adc_new_data_trigger = 0;
-               new_adc = 1; 
-    v1 = adc0_buf.sum / adc0_buf.av_nb_sample;
-    v2 = adc1_buf.values[0];
+       RunOnceEvery(2, {baro_periodic();});
 
-    RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &v1, &v2) });
+       if (adc_new_data_trigger) {
+               adc_new_data_trigger = 0;
+               new_adc = 1;
+               v1 = adc0_buf.sum / adc0_buf.av_nb_sample;
+               v2 = adc1_buf.values[0];
+
+               RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&v1, &v2) });
        }
 }
 
 static inline void on_rc_message(void) {
-  new_radio_msg = TRUE;
+       new_radio_msg = TRUE;
        if (radio_control.values[RADIO_CONTROL_MODE] >= 150) {
+#ifdef PASSTHROUGH_CYGNUS
+               autopilot_on_rc_frame();
+#else
                static int32_t commands[COMMANDS_NB];
                SetCommandsFromRC(commands, radio_control.values);
                SetActuatorsFromCommands(commands);
+#endif
        }
+#ifndef PASSTHROUGH_CYGNUS
        if (radio_control.values[RADIO_CONTROL_KILL] > 150) {
                actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN;
                ActuatorsCommit();
        }
+#endif
 }
 
 static inline void on_overo_link_msg_received(void) {
 
-  /* IMU up */ 
-  overo_link.up.msg.valid.imu = 1;
-  RATES_COPY(overo_link.up.msg.gyro, imu.gyro);
-  VECT3_COPY(overo_link.up.msg.accel, imu.accel);
-  VECT3_COPY(overo_link.up.msg.mag, imu.mag);
-  
-  /* RC up */
-  overo_link.up.msg.valid.rc  = new_radio_msg;
-  new_radio_msg = FALSE;
-  
-  overo_link.up.msg.rc_pitch  = radio_control.values[RADIO_CONTROL_PITCH];
-  overo_link.up.msg.rc_roll   = radio_control.values[RADIO_CONTROL_ROLL];
-  overo_link.up.msg.rc_yaw    = radio_control.values[RADIO_CONTROL_YAW];
-  overo_link.up.msg.rc_thrust = radio_control.values[RADIO_CONTROL_THROTTLE];
-  overo_link.up.msg.rc_mode   = radio_control.values[RADIO_CONTROL_MODE];
+       /* IMU up */
+       overo_link.up.msg.valid.imu = 1;
+       RATES_COPY(overo_link.up.msg.gyro, imu.gyro);
+       VECT3_COPY(overo_link.up.msg.accel, imu.accel);
+       VECT3_COPY(overo_link.up.msg.mag, imu.mag);
+
+       /* RC up */
+       overo_link.up.msg.valid.rc  = new_radio_msg;
+       new_radio_msg = FALSE;
+
+       overo_link.up.msg.rc_pitch  = radio_control.values[RADIO_CONTROL_PITCH];
+       overo_link.up.msg.rc_roll   = radio_control.values[RADIO_CONTROL_ROLL];
+       overo_link.up.msg.rc_yaw    = radio_control.values[RADIO_CONTROL_YAW];
+       overo_link.up.msg.rc_thrust = 
radio_control.values[RADIO_CONTROL_THROTTLE];
+       overo_link.up.msg.rc_mode   = radio_control.values[RADIO_CONTROL_MODE];
 #ifdef RADIO_CONTROL_KILL
-  overo_link.up.msg.rc_kill   = radio_control.values[RADIO_CONTROL_KILL];
+       overo_link.up.msg.rc_kill   = radio_control.values[RADIO_CONTROL_KILL];
 #endif
 #ifdef RADIO_CONTROL_GEAR
-  overo_link.up.msg.rc_gear   = radio_control.values[RADIO_CONTROL_GEAR];
+       overo_link.up.msg.rc_gear   = radio_control.values[RADIO_CONTROL_GEAR];
 #endif
-       
-  overo_link.up.msg.rc_aux2   = radio_control.values[RADIO_CONTROL_AUX2];
+
+       overo_link.up.msg.rc_aux2   = radio_control.values[RADIO_CONTROL_AUX2];
        overo_link.up.msg.rc_aux3   = radio_control.values[RADIO_CONTROL_AUX3];
        overo_link.up.msg.rc_status = radio_control.status;
-   
-  overo_link.up.msg.stm_msg_cnt     = overo_link.msg_cnt;
-  overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt;
-  
-  /* baro up */
-  overo_link.up.msg.valid.pressure_differential = new_baro_diff;
-  overo_link.up.msg.valid.pressure_absolute     = new_baro_abs;
-  new_baro_diff = FALSE;
-  new_baro_abs  = FALSE;
-  overo_link.up.msg.pressure_differential = baro.differential;
-  overo_link.up.msg.pressure_absolute     = baro.absolute;
-  
+
+       overo_link.up.msg.stm_msg_cnt     = overo_link.msg_cnt;
+       overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt;
+
+       /* baro up */
+       overo_link.up.msg.valid.pressure_differential = new_baro_diff;
+       overo_link.up.msg.valid.pressure_absolute     = new_baro_abs;
+       new_baro_diff = FALSE;
+       new_baro_abs  = FALSE;
+       overo_link.up.msg.pressure_differential = baro.differential;
+       overo_link.up.msg.pressure_absolute     = baro.absolute;
+
        /* vane up */
-  overo_link.up.msg.valid.vane =  new_vane;
-  new_vane = FALSE;
-  overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1;
-  overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2;
-  
+       overo_link.up.msg.valid.vane =  new_vane;
+       new_vane = FALSE;
+       overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1;
+       overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2;
+
        /* adc up */
-       overo_link.up.msg.adc.channels[0] = adc0_buf.sum / 
adc0_buf.av_nb_sample; 
-       overo_link.up.msg.adc.channels[1] = adc1_buf.sum / 
adc1_buf.av_nb_sample; 
-       overo_link.up.msg.adc.channels[2] = adc2_buf.sum / 
adc2_buf.av_nb_sample; 
-       overo_link.up.msg.adc.channels[3] = adc3_buf.sum / 
adc3_buf.av_nb_sample; 
-       overo_link.up.msg.valid.adc =  new_adc; 
+       overo_link.up.msg.adc.channels[0] = adc0_buf.sum / 
adc0_buf.av_nb_sample;
+       overo_link.up.msg.adc.channels[1] = adc1_buf.sum / 
adc1_buf.av_nb_sample;
+       overo_link.up.msg.adc.channels[2] = adc2_buf.sum / 
adc2_buf.av_nb_sample;
+       overo_link.up.msg.adc.channels[3] = adc3_buf.sum / 
adc3_buf.av_nb_sample;
+       overo_link.up.msg.valid.adc =  new_adc;
        new_adc = FALSE;
 
-  /* pwm acuators down */
+#ifdef PASSTHROUGH_CYGNUS
+       for (int i = 0; i < 6; i++) {
+               actuators_pwm_values[i] = 
overo_link.down.msg.pwm_outputs_usecs[i];
+       }
+       actuators_pwm_commit();
+
+       for (int i = 6; i < 10; i++) {
+               csc_servo_cmd.servos[i-6] = 
overo_link.down.msg.pwm_outputs_usecs[i];
+       }
+       cscp_transmit(0, CSC_SERVO_CMD_ID, (uint8_t *)&csc_servo_cmd, 
sizeof(csc_servo_cmd));
+#else
+       /* pwm acuators down */
        if (radio_control.values[RADIO_CONTROL_MODE] <= 150) {
-               for (int i = 0; i < LISA_PWM_OUTPUT_NB; i++) { 
+               for (int i = 0; i < 6; i++) { 
                        actuators_pwm_values[i] = 
overo_link.down.msg.pwm_outputs_usecs[i];
                }
                if (radio_control.values[RADIO_CONTROL_KILL] > 150) {
@@ -219,6 +274,7 @@
                }
                actuators_pwm_commit();
        }
+#endif
 }
 
 static inline void on_overo_link_lost(void) {
@@ -228,45 +284,61 @@
 }
 
 static inline void on_gyro_accel_event(void) {
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
+       ImuScaleGyro(imu);
+       ImuScaleAccel(imu);
+       overo_link.up.msg.imu_tick++;
+
+#ifdef PASSTHROUGH_CYGNUS
+       if (ahrs.status == AHRS_UNINIT) {
+               ahrs_aligner_run();
+               if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+                       ahrs_align();
+       } else {
+               ahrs_propagate();
+               ahrs_update_accel();
+               ins_propagate();
+       }
+#endif
 }
 
 static inline void on_mag_event(void) {
-  ImuScaleMag(imu);
+       ImuScaleMag(imu);
+
+#ifdef PASSTHROUGH_CYGNUS
+       if (ahrs.status == AHRS_RUNNING)
+               ahrs_update_mag();
+#endif
 }
 
-static inline void on_vane_msg(void *data) { 
-  new_vane = TRUE;
-  int zero = 0;
-  DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, 
-                           &(csc_vane_msg.vane_angle1), 
-                           &zero, 
-                           &zero, 
-                           &zero, 
-                           &zero, 
-                           &csc_vane_msg.vane_angle2, 
-                           &zero, 
-                           &zero, 
-                           &zero, 
-                           &zero);
+static inline void on_vane_msg(void *data) {
+       new_vane = TRUE;
+       int zero = 0;
+       DOWNLINK_SEND_VANE_SENSOR(DefaultChannel,
+                               &(csc_vane_msg.vane_angle1),
+                               &zero,
+                               &zero,
+                               &zero,
+                               &zero,
+                               &csc_vane_msg.vane_angle2,
+                               &zero,
+                               &zero,
+                               &zero,
+                               &zero);
 }
 
 static inline void main_on_baro_diff(void) {
-  new_baro_diff = TRUE;
+       new_baro_diff = TRUE;
 }
 
 static inline void main_on_baro_abs(void) {
-  new_baro_abs = TRUE;
+       new_baro_abs = TRUE;
 }
 
 static inline void main_event(void) {
-       
-  ImuEvent(on_gyro_accel_event, on_mag_event);
-  BaroEvent(main_on_baro_abs, main_on_baro_diff);
-  OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed);
-  RadioControlEvent(on_rc_message);
-  cscp_event();
+
+       ImuEvent(on_gyro_accel_event, on_mag_event);
+       BaroEvent(main_on_baro_abs, main_on_baro_diff);
+       OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed);
+       RadioControlEvent(on_rc_message);
+       cscp_event();
 }
-
-