Commit 9afc8dab authored by Timothy Stack's avatar Timothy Stack
Browse files

Add print outs for startnull and wheels commands. Add OnClearedFlags

option to mtp_dispatch.
parent 8def3310
......@@ -597,7 +597,7 @@ mtp_error_t mtp_init_packetv(struct mtp_packet *mp,
break;
}
break;
case MA_acceleration:
case MA_Acceleration:
switch (mp->data.opcode) {
case MTP_COMMAND_STARTNULL:
mp->data.mtp_payload_u.command_startnull.acceleration =
......@@ -899,6 +899,14 @@ int mtp_dispatch(void *userdata, mtp_packet_t *mp, mtp_dispatch_tag_t tag, ...)
if (!(tag & MD_OR))
value_curr += 1;
break;
case MD_OnClearedFlags:
assert(value_curr < value_count);
flags = va_arg(args, int);
match_curr = ((values[value_curr].i & flags) == 0);
if (!(tag & MD_OR))
value_curr += 1;
break;
case MD_OnOpcode:
match_curr = (mp->data.opcode == va_arg(args, mtp_opcode_t));
break;
......@@ -1215,6 +1223,30 @@ void mtp_print_packet(FILE *file, struct mtp_packet *mp)
mp->data.mtp_payload_u.command_stop.robot_id);
break;
case MTP_COMMAND_STARTNULL:
fprintf(file,
" opcode:\tcommand-startnull\n"
" commid:\t%d\n"
" id:\t%d\n"
" accel:\t%.3f\n",
mp->data.mtp_payload_u.command_startnull.command_id,
mp->data.mtp_payload_u.command_startnull.robot_id,
(double)mp->data.mtp_payload_u.command_startnull.acceleration);
break;
case MTP_COMMAND_WHEELS:
fprintf(file,
" opcode:\tcommand-wheels\n"
" commid:\t%d\n"
" robot:\t%d\n"
" vleft:\t%.3f\n"
" vright:\t%.3f\n",
mp->data.mtp_payload_u.command_wheels.command_id,
mp->data.mtp_payload_u.command_wheels.robot_id,
mp->data.mtp_payload_u.command_wheels.vleft,
mp->data.mtp_payload_u.command_wheels.vright);
break;
case MTP_TELEMETRY:
fprintf(file,
" opcode:\ttelemetry - %d\n",
......
......@@ -197,7 +197,7 @@ typedef enum {
MA_Theta, /*< (double) */
MA_vleft, /*< (float) */
MA_vright, /*< (float) */
MA_acceleration, /*< (float) */
MA_Acceleration, /*< (float) */
MA_Timestamp, /*< (double) */
MA_Status, /*< (mtp_status_t) */
MA_RequestID, /*< (int) */
......@@ -315,6 +315,7 @@ typedef enum {
MD_OnInteger, /*< (int) Check an MD_Integer. */
MD_OnFlags, /*< (int) Bitwise check of an MD_Integer. */
MD_OnClearedFlags, /*< (int) Bitwise check of an MD_Integer. */
MD_SkipInteger, /*< (void) Skip checking an MD_Integer. */
MD_Return, /*< (void) On match, return immediately. */
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment