X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/blobdiff_plain/893cf3d29c318d7444e7caa1609ebbf7948636be..16d700e75c6c24612de49c1370eb6c293de695df:/src/cand/cand.cc

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index ea2cbd08..c36836da 100644
--- 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);