changed pos_speed_est_neu update rate to 50Hz

chasenmoses · Jan 1, 2000 · 03b18a0 · 03b18a0
1 parent 575b9d2
commit 03b18a0
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 13 deletions.
diff --git a/pos_speed_est_neu/service/main.c b/pos_speed_est_neu/service/main.c
@@ -113,17 +113,19 @@ SERVICE_MAIN_BEGIN("pos_speed_est_neu", PP_PRIO_3)
 
          /* run position estimate: */
          pos_update(&pos, &pos_in);
-         msgpack_sbuffer_clear(msgpack_buf);
-         msgpack_pack_array(pk, 8);
-         PACKF(pos.ultra_u.pos);
-         PACKF(pos.ultra_u.speed);
-         PACKF(pos.baro_u.pos);
-         PACKF(pos.baro_u.speed);
-         PACKF(pos.ne_pos.n);
-         PACKF(pos.ne_speed.n);
-         PACKF(pos.ne_pos.e);
-         PACKF(pos.ne_speed.e);
-         scl_copy_send_dynamic(pos_speed_est_socket, msgpack_buf->data, msgpack_buf->size);
+         EVERY_N_TIMES(4, /* ~50 Hz: */ 
+            msgpack_sbuffer_clear(msgpack_buf);
+            msgpack_pack_array(pk, 8);
+            PACKF(pos.ultra_u.pos);
+            PACKF(pos.ultra_u.speed);
+            PACKF(pos.baro_u.pos);
+            PACKF(pos.baro_u.speed);
+            PACKF(pos.ne_pos.n);
+            PACKF(pos.ne_speed.n);
+            PACKF(pos.ne_pos.e);
+            PACKF(pos.ne_speed.e);
+            scl_copy_send_dynamic(pos_speed_est_socket, msgpack_buf->data, msgpack_buf->size);
+         )
       }
    }
    MSGPACK_READER_SIMPLE_LOOP_END;

diff --git a/scl/shared/scl.py b/scl/shared/scl.py
@@ -43,10 +43,12 @@ def scl_get_socket(id, type_name):
    socket_path = pp_path + id
    socket = context.socket(socket_type)
    if socket_type in [zmq.SUB, zmq.REQ, zmq.PUSH]:
+      socket.setsockopt(zmq.RCVHWM, 1)
       if socket_type == zmq.SUB:
          socket.setsockopt(zmq.SUBSCRIBE, "")
       socket.connect(socket_path)
    elif socket_type in [zmq.PUB, zmq.REP, zmq.PULL]:
+      socket.setsockopt(zmq.SNDHWM, 1)
       socket.bind(socket_path)
    else:
       raise Exception("unknown socket type: %d" % socket_type)

diff --git a/test_programs/rc_hs_test.py b/test_programs/rc_hs_test.py
@@ -70,8 +70,8 @@
          yaw_speed_socket.send(dumps(0.6 * yaw))
       else:
          mot_en_socket.send(dumps(0))
-except:
-   pass
+except Exception, e:
+   print e
 
 mot_en_socket.send(dumps(0))
 sleep(0.5)