[paparazzi-commits] [5213] some changes to fix after refactoring.

[Paul Cox]

Revision: 5213
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5213
Author:   paulcox
Date:     2010-08-04 15:56:02 +0000 (Wed, 04 Aug 2010)
Log Message:
-----------
some changes to fix after refactoring. datalink/telemetry and control working.

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/beth/main_overo.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c
    paparazzi3/trunk/sw/airborne/beth/overo_controller.c
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
    paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h

Modified: paparazzi3/trunk/sw/airborne/beth/main_overo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-04 13:55:19 UTC 
(rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/main_overo.c      2010-08-04 15:56:02 UTC 
(rev 5213)
@@ -44,12 +44,6 @@
 #include "overo_controller.h"
 
 
-
-#define GCS_HOST "10.31.4.104"
-#define GCS_PORT 4242
-#define DATALINK_PORT 4243
-
-
 static void main_periodic(int);
 static void main_parse_cmd_line(int argc, char *argv[]);
 static void main_exit(int sig);
@@ -67,7 +61,6 @@
 
 
 int main(int argc, char *argv[]) {
-
   
   (void) signal(SIGINT, main_exit);
   
@@ -90,7 +83,7 @@
 
   //  file_logger_init("my_log.data");
 
-  gcs_com_init(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE);
+  gcs_com_init();
 
   main_parse_cmd_line(argc, argv);
   
@@ -104,11 +97,20 @@
 
 
 static void main_periodic(int my_sig_num) {
+#if 0
+DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);
+main_talk_with_stm32();
+estimator_run(msg_in.bench_sensor.z);
+control_run();
+BoozImuScaleGyro();
+gcs_com_periodic();
+#endif
 
+#if 01 
   RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);});
-  
+ 
   main_talk_with_stm32();
-  
+
   RunOnceEvery(50, 
{DOWNLINK_SEND_BETH(gcs_com.udp_transport,&msg_in.bench_sensor.x,&msg_in.bench_sensor.y,
                                       &msg_in.bench_sensor.z,&foo);});
   
@@ -140,8 +142,8 @@
 
   //RunOnceEvery(33, {UdpTransportPeriodic();});
   RunOnceEvery(33, gcs_com_periodic());
+#endif
 
-
 }
 
 
@@ -186,6 +188,7 @@
     printf("cmd now %f\n",controller.cmd);
   }
   msg_out.pitch = (int8_t)controller.cmd;
+  //msg_out.pitch = 1;
   //msg_out.cksum = msg_out.thrust + msg_out.pitch;
 
   spi_link_send(&msg_out, sizeof(union AutopilotMessage) , &msg_in);

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-04 13:55:19 UTC 
(rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-04 15:56:02 UTC 
(rev 5213)
@@ -96,10 +96,10 @@
   booz2_commands[COMMAND_PITCH] = pitch;
   booz2_commands[COMMAND_ROLL] = 0;
   booz2_commands[COMMAND_YAW] = 0;
-  if ( overo_link.down.msg.thrust < 10) {
+  if ( overo_link.down.msg.thrust < 20) {
     booz2_commands[COMMAND_THRUST] = overo_link.down.msg.thrust;
   } else { 
-    booz2_commands[COMMAND_THRUST] = 10;
+    booz2_commands[COMMAND_THRUST] = 20;
   }
   if (my_cnt == 0) {
     actuators_set(FALSE);

Modified: paparazzi3/trunk/sw/airborne/beth/overo_controller.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-04 
13:55:19 UTC (rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/overo_controller.c        2010-08-04 
15:56:02 UTC (rev 5213)
@@ -16,10 +16,14 @@
 
 
 void control_run(void) {
-  
+  static int foo=0;
+
   float track_err = estimator.tilt - controller.tilt_sp;
-  
-  controller.cmd = controller.kp*track_err + controller.kd*estimator.tilt_dot;
-  
+  float pcmd = controller.kp*track_err;
+  float dcmd = controller.kd*estimator.tilt_dot;
+  //controller.cmd = controller.kp*track_err + 
controller.kd*estimator.tilt_dot;
+  controller.cmd = pcmd + dcmd;
+  //if (!(foo%100)) printf("%f %f\n",pcmd,dcmd);
+  foo++;
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-04 13:55:19 UTC 
(rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-04 15:56:02 UTC 
(rev 5213)
@@ -11,10 +11,11 @@
 
 void estimator_run(uint16_t tilt_measure) {
   
-  const int32_t tilt_neutral = 2770;
-  const float   tilt_scale = 1./580.;
+  const int32_t tilt_neutral = 2815;
+  //const float   tilt_scale = 1./580.;
+  const float   tilt_scale = 1.;
 
-  estimator.tilt = ((int32_t)tilt_measure - tilt_neutral) * tilt_scale;
+  estimator.tilt = (tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
   estimator.tilt_dot = booz_imu.gyro.q;
 
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-04 13:55:19 UTC 
(rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.c   2010-08-04 15:56:02 UTC 
(rev 5213)
@@ -1,6 +1,5 @@
 #include "overo_gcs_com.h"
 
-#include <event.h>
 #include <string.h>
 #include <unistd.h>
 
@@ -11,6 +10,9 @@
 #include "settings.h"
 //#include "downlink.h"
 
+#define GCS_HOST "10.31.4.104"
+#define GCS_PORT 4242
+#define DATALINK_PORT 4243
 
 static void dl_handle_msg(struct DownlinkTransport *tp);
 static inline int checked_read(int fd, char *buf, size_t len);
@@ -18,15 +20,14 @@
 
 struct OveroGcsCom gcs_com;
 
+void gcs_com_init(void) {
 
-void gcs_com_init(char* gcs_host, uint32_t gcs_port, uint32_t datalink_port, 
uint8_t broadcast) {
-
-  gcs_com.network = network_new(gcs_host, gcs_port, datalink_port, broadcast);
+  gcs_com.network = network_new(GCS_HOST,GCS_PORT,DATALINK_PORT,FALSE);
   gcs_com.udp_transport = udp_transport_new(gcs_com.network);
-  struct event datalink_event;
-  event_set(&datalink_event, gcs_com.network->socket_in, EV_READ| EV_PERSIST, 
on_datalink_event, gcs_com.udp_transport);
-  event_add(&datalink_event, NULL);
   
+  event_set(&gcs_com.datalink_event, gcs_com.network->socket_in, EV_READ| 
EV_PERSIST, on_datalink_event, gcs_com.udp_transport);
+  event_add(&gcs_com.datalink_event, NULL);
+  
 }
 
 void gcs_com_periodic(void) {
@@ -70,7 +71,8 @@
     {
       uint8_t i = DL_SETTING_index(gcs_com.my_dl_buffer);
       float var = DL_SETTING_value(gcs_com.my_dl_buffer);
-      DlSetting(i, var);
+      //DlSetting(i, var);
+      printf("datalink : %d %f\n",i,var);
       DOWNLINK_SEND_DL_VALUE(tp, &i, &var);
     }
     break;

Modified: paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h   2010-08-04 13:55:19 UTC 
(rev 5212)
+++ paparazzi3/trunk/sw/airborne/beth/overo_gcs_com.h   2010-08-04 15:56:02 UTC 
(rev 5213)
@@ -1,6 +1,7 @@
 #ifndef OVERO_GCS_COM_H
 #define OVERO_GCS_COM_H
 
+#include <event.h>
 #include "fms_network.h"
 #include "downlink_transport.h"
 
@@ -12,6 +13,7 @@
 
   struct FmsNetwork* network;
   struct DownlinkTransport *udp_transport;
+  struct event datalink_event;
   /*  bool_t  my_dl_msg_available; */
   uint8_t my_dl_buffer[GCS_COM_DL_BUF_SIZE]  __attribute__ ((aligned));
 
@@ -20,8 +22,7 @@
 
 extern struct OveroGcsCom gcs_com;
 
-
-extern void gcs_com_init(char* gcs_host, uint32_t gcs_port, uint32_t 
datalink_port, uint8_t broadcast);
+extern void gcs_com_init(void);
 extern void gcs_com_periodic(void);