Unify ORTE initialization

[eurobot/public.git] / src / cand / cand.cc

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index ea2cbd081226a1b04658ccdc3a0462828f25c1bf..c36836dad572885e3d134a50da40117f8dabef21 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -86,16 +86,6 @@ int set_pwr_ctrl(struct robottype_orte_data *orte_data)
        return 0;
 }
 
-int set_pwr_alert(struct robottype_orte_data *orte_data)
-{
-       unsigned char data=0;
-       data = orte_data->pwr_alert.value;
-
-       can_send(CAN_PWR_ALERT, 1, &data);
-
-       return 0;
-}
-
 int send_can_msg(struct robottype_orte_data *orte_data)
 {
        return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
@@ -174,8 +164,6 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
        static int status_cnt = 0;
 
        switch(frame.can_id) {
-               /* voltage measurements from power board */
-
                /* robot commands (start, ..) */
                case CAN_JAW_LEFT_STATUS:
                        orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
@@ -252,6 +240,7 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
                                status_cnt = 0;
                        }
                        break;
+                /* voltage measurements from power board */
                case CAN_PWR_ADC1:
                        double volt33, voltBAT;
                        voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
@@ -274,6 +263,19 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
                        ORTEPublicationSend(orte->publication_pwr_voltage);
                        
                        break;
+
+                case CAN_PWR_ALERT:
+                        orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
+                        orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
+                        orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
+                        orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
+                        orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
+                        orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
+                        orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
+
+                        ORTEPublicationSend(orte->publication_pwr_alert);
+                        break;
+
                default:
                        //FIXME: add logging here (Filip)
 //                     printf("received CAN msg with unknown id: %x\n",frame.can_id);
@@ -310,20 +312,6 @@ void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
                        break;
        }
 }
-void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance, 
-                       void *recvCallBackParam) 
-{
-
-       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
-       switch (info->status) {
-               case NEW_DATA:
-                       set_pwr_alert(orte_data);
-                       break;
-               case DEADLINE:
-                       //printf("ORTE deadline occurred - PWR_CTRL receive\n");
-                       break;
-       }
-}
 
 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, 
                                void *recvCallBackParam)
@@ -375,12 +363,13 @@ void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 }
 }
 
+struct robottype_orte_data orte;
+
 int main(int argc, char *argv[])
 {
        int ret;
        int size;
 
-       struct robottype_orte_data orte;
        struct can_frame frame;
 
        if (cand_init() < 0) {
@@ -390,8 +379,6 @@ int main(int argc, char *argv[])
                printf("cand: init OK\n");
        }
 
-       orte.strength = 1;
-
        /* orte initialization */
        if(robottype_roboorte_init(&orte)) {
                printf("Roboorte initialization failed! Exiting...\n");
@@ -414,7 +401,6 @@ int main(int argc, char *argv[])
 
        /* creating subscribers */
        robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
-       robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
        robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
        robottype_subscriber_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
         robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);