robofsm and ulcdd: added support for HW units status reporting

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index 2e1aa4e8b1b641072b5b29c6a05f59b9904ffec6..6af5562f9b8a638fe202e2e726cde76dea8630f8 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -28,6 +28,7 @@
 
 struct sercom_data *sercom;
 uint8_t msg[10];
+uint8_t hw_status[6];
 bool msg_waiting;
 bool wait_flag;
 uint8_t mode_to_go;

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index a35512e3f8a4a4be3dc17917bf5edbc17c289ada..2d236f9ba062473e7cf4a59dd25d64e81e65fdbc 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -182,6 +182,9 @@ struct robot_eb2008 {
        struct mcl_laser laser; /* MCL implementation for laser */
 
        struct map *map;        /* Map for pathplanning (no locking) */
+
+       /* information about HW units status - for display */
+       uint8_t hw_status[6];
 }; /* robot_eb2008 */
 
 extern struct robot_eb2008 robot;

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index e7fc7b0354b909daa8c8f6ad71e9b608bbefb799..a9fd32113f26f776a8323700209193dd6144e084 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -17,7 +17,7 @@
 #include <math.h>
 #include <robomath.h>
 #include <laser-nav.h>
-
+#include <oledlib.h>
 /* ---------------------------------------------------------------------- 
  * PUBLISHER CALLBACKS - GENERIC
  * ---------------------------------------------------------------------- */
@@ -87,8 +87,10 @@ void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
                        odo->dy = 0;
                        odo->dangle = deltAlfa;
                        FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
                        break;
                case DEADLINE:
+                       robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
                        DBG("ORTE deadline occurred - motion_irc receive\n");
                        break;
        }
@@ -140,8 +142,10 @@ void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
 {
        switch (info->status) {
                case NEW_DATA:
+                       robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
                        break;
                case DEADLINE:
+                       robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
                        DBG("ORTE deadline occurred - pwr_voltage receive\n");
                        break;
        }
@@ -215,8 +219,10 @@ void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
                                free(meas_angles);
                        }
                        break;
+                       robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
                }
                case DEADLINE:
+                       robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
                        DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
                        break;
        }
@@ -235,8 +241,10 @@ void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
                        if (robot.cmu.color != NO_BALL)
                                FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
                        break;
+                       robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
                }
                case DEADLINE:
+                       robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
                        DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
                        break;
        }

diff --git a/src/ulcdd/oledlib.cc b/src/ulcdd/oledlib.cc
index 0f30e0c1102095b87e4f2629300c47eccca7fc8d..7d966d07f6be252dde993aee26ac5a91e30374ee 100644 (file)
--- a/src/ulcdd/oledlib.cc
+++ b/src/ulcdd/oledlib.cc
@@ -31,6 +31,24 @@ int oled_set_balls(uint8_t *buff, int buff_size, uint8_t *carousel)
        return 0;
 }
 
+int oled_send_hw_status(uint8_t *buff, int buff_size, uint8_t *status)
+{
+       if(buff_size < HW_STATUS_MSG_SIZE)
+               return -1;
+       
+       buff[0] = MSG_START;
+       buff[1] = HW_STATUS_MSG;
+       buff[2] = status[0];
+       buff[3] = status[1];
+       buff[4] = status[2];
+       buff[5] = status[3];
+       buff[6] = status[4];
+       buff[7] = status[5];
+       buff[8] = MSG_TERM;
+       printf("\n");   
+       return 0;
+}
+
 int oled_set_color(uint8_t *buff, int buff_size, uint8_t color)
 {
        if(buff_size < COLOR_MSG_SIZE)

diff --git a/src/ulcdd/oledlib.h b/src/ulcdd/oledlib.h
index 6ce91e26a4b24e1d40cfe53a4c2e668cf77e2996..5d0f15bfda279d9fe9af466975e23d757542c4c0 100644 (file)
--- a/src/ulcdd/oledlib.h
+++ b/src/ulcdd/oledlib.h
@@ -5,6 +5,7 @@
 #include <robottype.h>
 #include <robottype_eb2008.h>
 
+#define HW_STATUS_MSG_SIZE 9
 #define SWITCH_MODE_MSG_SIZE 5
 #define VOLTAGE_MSG_SIZE 20 
 #define FSM_STATE_MSG_SIZE 40
@@ -55,6 +56,13 @@
 #define RED_BALL 2
 #define NO_BALL 3
 #define HW_STATUS_OK 1
+#define HW_STATUS_FAILED 0
+
+#define STATUS_MOTION 0
+#define STATUS_LASER 1
+#define STATUS_CMU 2
+#define STATUS_POWER 3
+
 
 struct mode_info {
        uint8_t mode;
@@ -77,6 +85,7 @@ int oled_set_color(uint8_t *buff, int buff_size, uint8_t color);
 int oled_send_position(uint8_t *buff, int buff_size, struct est_pos_type *pos);
 int oled_send_fsm_state(uint8_t *buff, int buff_size, const char *name, int len);
 int oled_set_balls(uint8_t *buff, int buff_size, uint8_t *carousel);
+int oled_send_hw_status(uint8_t *buff, int buff_size, uint8_t *status);
 #ifdef __cplusplus
 }
 #endif

diff --git a/src/ulcdd/uoled.c b/src/ulcdd/uoled.c
index 763c5384782a39f60c0fb937b3bcd0550ea0f4b0..28148d4d0c3e8b9e99d44d73269ed8c7d78a06aa 100644 (file)
--- a/src/ulcdd/uoled.c
+++ b/src/ulcdd/uoled.c
@@ -27,7 +27,6 @@ struct sercom_data* uoled_init(void(*sighandler)(int))
        sercom.stopbits = 1;
        sercom.mode = 0;
        sercom.sighandler = sighandler;
-//     sercom.saio.sa_handler = lcd_receive;
        sercom_open(&sercom);
        return &sercom;
 }
@@ -123,6 +122,21 @@ int uoled_send_position(struct est_pos_type *pos)
 
        return 0;
 }
+
+
+int uoled_send_hw_status(uint8_t *status) 
+{
+       int ret;
+       uint8_t msg[HW_STATUS_MSG_SIZE];
+
+       oled_send_hw_status(msg, HW_STATUS_MSG_SIZE, status);
+
+       ret = uoled_write_cmd(msg, HW_STATUS_MSG_SIZE);
+       if(ret)
+               return -1;
+
+       return 0;
+}
 int uoled_send_state(const char *name, int len)
 {
        int ret;

diff --git a/src/ulcdd/uoled.h b/src/ulcdd/uoled.h
index 5d861d9fb72a53b9aa55727c7bc2e42a936533e0..e544269acd9659f870fbddf48b9564071b68f607 100644 (file)
--- a/src/ulcdd/uoled.h
+++ b/src/ulcdd/uoled.h
@@ -14,6 +14,7 @@ int uoled_send_state(const char *name, int len);
 int uoled_send_position(struct est_pos_type *pos); 
 int uoled_set_color(uint8_t color);
 int uoled_set_balls(uint8_t *balls); 
+int uoled_send_hw_status(uint8_t *status);
 #ifdef __cplusplus
 }
 #endif