cand: Fix bug with power alert handling.

I think the previous version is wrong - PWR board sends CAN msgs when voltages are out of limits.
But CAN daemon do not receive and parse msgs with this alert ID.
Instead can daemon received alert msgs from ORTE and forwarded them to CAN.

Now I fixed this and alert msgs from PWR board are parsed and forwarded to ORTE.

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index ea2cbd081226a1b04658ccdc3a0462828f25c1bf..dc3920e016159b0bd81cb880cc6ed057f469d8c1 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)