[paparazzi-commits] [5461] CSC Updates from Tobias

[Allen Ibara]

Revision: 5461
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5461
Author:   aibara
Date:     2010-08-19 03:18:01 +0000 (Thu, 19 Aug 2010)
Log Message:
-----------
CSC Updates from Tobias

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/csc/csc_protocol.c
    paparazzi3/trunk/sw/airborne/csc/csc_protocol.h
    paparazzi3/trunk/sw/airborne/csc/csc_vane.c
    paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c

Modified: paparazzi3/trunk/sw/airborne/csc/csc_protocol.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_protocol.c     2010-08-18 16:55:29 UTC 
(rev 5460)
+++ paparazzi3/trunk/sw/airborne/csc/csc_protocol.c     2010-08-19 03:18:01 UTC 
(rev 5461)
@@ -29,6 +29,9 @@
 #include "can.h"
 #include "csc_msg_def.h"
 
+// #include "init_hw.h"
+// #include "sys_time.h"
+// #include "downlink.h"
 
 #define CSCP_QUEUE_LEN 8
 
@@ -42,6 +45,7 @@
        int head;
        int tail;
        int full;
+       int empty; 
        struct cscp_msg msgs[CSCP_QUEUE_LEN];
 } cscp_msg_queue;
 
@@ -93,10 +97,11 @@
        /* just store the incoming data in a buffer with very little processing 
*/
 
        /* TODO we should handle the return value of enqueue somehow,
-        * the returnvalue tells us if the queue had enough space to
+        * the return value tells us if the queue had enough space to
         * store our message or not
         */
        uint32_t msg_id = cscp_msg_id(id);
+
        cscp_enqueue(msg_id, buf, len);
 }
 
@@ -105,45 +110,51 @@
        /* this periodig is the main csc protocol event dispatcher */
        int msg_id = cscp_peek_msg_id();
 
-       if(msg_id == -1){
+       if (msg_id == -1){
                return;
        }
 
        if(cscp_callback_handles[msg_id].callback){
                cscp_dequeue(cscp_callback_handles[msg_id].data);
                
cscp_callback_handles[msg_id].callback(cscp_callback_handles[msg_id].data);
+               return;
        }
 
 }
 
 void cscp_queue_init(void)
 {
-       cscp_msg_queue.head = 0;
-       cscp_msg_queue.tail = 0;
-       cscp_msg_queue.full = 0;
+       cscp_msg_queue.head  = 0;
+       cscp_msg_queue.tail  = 0;
+       cscp_msg_queue.full  = 0;
+       cscp_msg_queue.empty = 1;
 }
 
 int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len)
 {
        if (cscp_msg_queue.full) {
-               return 1;
+               cscp_msg_queue.empty = 0; 
+               return 1; 
        }
+       if(!cscp_callback_handles[msg_id].callback) {
+               return 2; 
+       }
 
        cscp_msg_queue.msgs[cscp_msg_queue.tail].id = msg_id;
        cscp_msg_queue.msgs[cscp_msg_queue.tail].len = len;
        memcpy(cscp_msg_queue.msgs[cscp_msg_queue.tail].data, buf, len);
-       cscp_msg_queue.tail = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN;
-       if (cscp_msg_queue.head == cscp_msg_queue.tail)
+       cscp_msg_queue.tail  = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN;
+       cscp_msg_queue.empty = 0; 
+       if (cscp_msg_queue.head == cscp_msg_queue.tail) {
                cscp_msg_queue.full = 1;
+       }
 
        return 0;
 }
 
 int cscp_peek_msg_id(void)
 {
-       if(!cscp_msg_queue.full &&
-               (cscp_msg_queue.head == cscp_msg_queue.tail))
-       {
+       if (cscp_msg_queue.empty) {
                return -1;
        }
        return cscp_msg_queue.msgs[cscp_msg_queue.head].id;
@@ -151,8 +162,7 @@
 
 int cscp_dequeue(uint8_t *buf)
 {
-       if (!cscp_msg_queue.full &&
-               (cscp_msg_queue.head == cscp_msg_queue.tail)) {
+       if (cscp_msg_queue.empty) {
                return 1;
        }
 
@@ -161,6 +171,9 @@
                cscp_msg_queue.msgs[cscp_msg_queue.head].len);
        cscp_msg_queue.head = (cscp_msg_queue.head + 1) % CSCP_QUEUE_LEN;
        cscp_msg_queue.full = 0;
+       if (cscp_msg_queue.head == cscp_msg_queue.tail) { 
+               cscp_msg_queue.empty = 1; 
+       }
 
        return 0;
 }

Modified: paparazzi3/trunk/sw/airborne/csc/csc_protocol.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_protocol.h     2010-08-18 16:55:29 UTC 
(rev 5460)
+++ paparazzi3/trunk/sw/airborne/csc/csc_protocol.h     2010-08-19 03:18:01 UTC 
(rev 5461)
@@ -26,10 +26,19 @@
 #define CSC_PROTOCOL_H
 
 typedef void(* cscp_callback_t)(void *data);
+typedef enum { INIT=0, READ=1, WRITE=2 } queue_state_t; 
 
 void cscp_init(void);
 int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, 
uint8_t len);
 void cscp_event(void);
+
+/* Note that messages are only accepted and parsed if a callback function 
+ * has been registered for their respective message id. 
+ * Example: 
+ *
+ *             cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void 
*)&csc_vane_msg);
+ * 
+ */
 void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t 
*data);
 
 #endif

Modified: paparazzi3/trunk/sw/airborne/csc/csc_vane.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_vane.c 2010-08-18 16:55:29 UTC (rev 
5460)
+++ paparazzi3/trunk/sw/airborne/csc/csc_vane.c 2010-08-19 03:18:01 UTC (rev 
5461)
@@ -22,7 +22,9 @@
   static uint8_t prescale = 0;
 
   for (int i = 0; i < VANE_NB; i++) {
-    angle[i] = avg_factor * angle[i] + (1. - avg_factor) * RadOfDeg(360. * 
pwm_input_duration[i] / PWM_INPUT_COUNTS_PER_REV);
+    angle[i] = avg_factor * angle[i] + 
+                                                               ((1. - 
avg_factor) * 
+                                                                RadOfDeg(360. 
* pwm_input_duration[i] / PWM_INPUT_COUNTS_PER_REV));
   }
 
   if (prescale++ >= PWM_INPUT_MSG_PRESCALE) {

Modified: paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2010-08-18 16:55:29 UTC 
(rev 5460)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2010-08-19 03:18:01 UTC 
(rev 5461)
@@ -105,9 +105,13 @@
 
 
 // be sure to call servos_init after uart1 init since they are sharing pins
+       csc_servos_init();
+
 #ifdef USE_VANE_SENSOR
-       csc_servos_init();
+#ifndef USE_PWM_INPUT
+       pwm_input_init();
 #endif
+#endif
 
        csc_ap_link_init();
        csc_ap_link_set_servo_cmd_cb(on_servo_cmd);