cand: Add odo_data handling

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index f986f8b08235ae26425c5ff836ab84540d7167c0..ac24c29ba34e1663e69ea5ed22c6f6408382baa5 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -180,6 +180,19 @@ int cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
                        ORTEPublicationSend(orte->publication_corr_distances);
                        break;
 
+               /* positioning by odometry */
+               case CAN_ODO_DATA:
+                       orte->odo_data.right = 
+                                       ((frame.data[0]<<24)|
+                                        (frame.data[1]<<16)|
+                                        (frame.data[2]<<8));
+                       orte->odo_data.left = 
+                                       ((frame.data[3]<<24)|
+                                        (frame.data[4]<<16)|
+                                        (frame.data[5]<<8));
+                       ORTEPublicationSend(orte->publication_odo_data);
+                       break;
+
                /* positioning by odometry */
                case CAN_MOTION_ODOMETRY_SIMPLE:
                        orte->motion_irc.right = 
@@ -346,6 +359,7 @@ int main(int argc, char *argv[])
        /* creating publishers */
        robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
        robottype_publisher_motion_status_create(&orte, NULL, NULL);
+       robottype_publisher_odo_data_create(&orte, NULL, NULL);
        robottype_publisher_motion_irc_create(&orte, NULL, NULL);
        robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
        robottype_publisher_corr_trig_create(&orte, NULL, NULL);