robofsm: update robot start event.

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 7d8b50928d3f29fc22aa5de05153ba94491afe93..f2356b6ff739cda26eba6f94db676eaa77957622 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -165,6 +165,25 @@ void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
+void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+                       void *recvCallBackParam)
+{
+       struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
+       static unsigned char start_received = 0;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       if (instance->start && !start_received) {
+                               FSM_SIGNAL(MAIN, EV_START, NULL);
+                               start_received = 1;
+                       }
+                       break;
+               case DEADLINE:
+                       DBG("ORTE deadline occurred - pwr_ctrl receive\n");
+                       break;
+       }
+}
+
 /* ---------------------------------------------------------------------- 
  * SUBSCRIBER CALLBACKS - EB2008
  * ---------------------------------------------------------------------- */
@@ -326,6 +345,7 @@ void robot_init_orte()
        generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
        generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
        generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
+       generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
 
        /* create eb2008 subscribers */
        eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);