pbio/control: log control instead of servo and drivebase

Attach logger to control object instead of to the servo
and the drivebase separately. This makes it possible to log
additional control data and reduces duplicate code.

Also reduces move hub firmware size by several hundred bytes.

lib/pbio/include/pbio/control.h

@@ -13,6 +13,7 @@
 #include <pbio/port.h>
 #include <pbio/trajectory.h>
 #include <pbio/integrator.h>
+#include <pbio/logger.h>
 
 #include <pbio/iodev.h>
 
@@ -73,6 +74,7 @@ typedef struct _pbio_control_t {
     pbio_rate_integrator_t rate_integrator;
     pbio_count_integrator_t count_integrator;
     pbio_control_on_target_t on_target_func;
+    pbio_log_t log;
     bool stalled;
     bool on_target;
 } pbio_control_t;

lib/pbio/include/pbio/drivebase.h

@@ -13,7 +13,6 @@ typedef struct _pbio_drivebase_t {
     pbio_servo_t *right;
     int32_t sum_offset;
     int32_t dif_offset;
-    pbio_log_t log;
     pbio_control_t control_heading;
     pbio_control_t control_distance;
 } pbio_drivebase_t;

lib/pbio/include/pbio/logger.h

@@ -13,7 +13,7 @@
 #define NUM_DEFAULT_LOG_VALUES (1)
 
 // Maximum number of values to be logged per sample
-#define MAX_LOG_VALUES (20)
+#define MAX_LOG_VALUES (NUM_DEFAULT_LOG_VALUES + 7)
 
 typedef struct _pbio_log_t {
     bool active;

lib/pbio/include/pbio/servo.h

@@ -31,7 +31,6 @@ typedef struct _pbio_servo_t {
     pbio_tacho_t *tacho;
     pbio_control_t control;
     pbio_port_t port;
-    pbio_log_t log;
 } pbio_servo_t;
 
 pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix16_t gear_ratio);

lib/pbio/src/control.c

@@ -106,6 +106,18 @@ void control_update(pbio_control_t *ctl, int32_t time_now, int32_t count_now, in
         *actuation_type = PBIO_ACTUATION_DUTY;
         *control = duty;
     }
+
+    // Log control data
+    int32_t log_data[] = {
+        (time_ref - ctl->trajectory.t0) / 1000,
+        count_now,
+        rate_now,
+        PBIO_ACTUATION_DUTY,
+        *control,
+        count_ref,
+        rate_ref,
+    };
+    pbio_logger_update(&ctl->log, log_data);
 }
 
 

lib/pbio/src/drivebase.c

@@ -8,8 +8,6 @@
 #include <pbio/math.h>
 #include <pbio/servo.h>
 
-#define DRIVEBASE_LOG_NUM_VALUES (15 + NUM_DEFAULT_LOG_VALUES)
-
 #if PBDRV_CONFIG_NUM_MOTOR_CONTROLLER != 0
 
 static pbio_error_t drivebase_adopt_settings(pbio_control_settings_t *s_distance, pbio_control_settings_t *s_heading, pbio_control_settings_t *s_left, pbio_control_settings_t *s_right) {
@@ -142,44 +140,6 @@ static pbio_error_t pbio_drivebase_actuate(pbio_drivebase_t *db, pbio_actuation_
     return err;
 }
 
-// Log motor data for a motor that is being actively controlled
-static pbio_error_t drivebase_log_update(pbio_drivebase_t *db,
-    int32_t time_now,
-    int32_t sum,
-    int32_t sum_rate,
-    int32_t sum_control,
-    int32_t dif,
-    int32_t dif_rate,
-    int32_t dif_control) {
-
-    int32_t buf[DRIVEBASE_LOG_NUM_VALUES];
-    buf[0] = time_now;
-    buf[1] = sum;
-    buf[2] = sum_rate;
-    buf[3] = sum_control;
-    buf[4] = dif;
-    buf[5] = dif_rate;
-    buf[6] = dif_control;
-
-    int32_t sum_ref, sum_ref_ext, sum_rate_ref, sum_rate_err, sum_rate_err_integral, sum_acceleration_ref;
-    pbio_trajectory_get_reference(&db->control_distance.trajectory, time_now, &sum_ref, &sum_ref_ext, &sum_rate_ref, &sum_acceleration_ref);
-    pbio_rate_integrator_get_errors(&db->control_distance.rate_integrator, time_now, sum_rate_ref, sum, sum_ref, &sum_rate_err, &sum_rate_err_integral);
-    buf[7] = sum_ref;
-    buf[8] = sum_rate_err_integral;
-    buf[9] = sum_rate_ref;
-    buf[10] = sum_rate_err_integral;
-
-    int32_t dif_ref, dif_ref_ext, dif_rate_ref, dif_rate_err, dif_rate_err_integral, dif_acceleration_ref;
-    pbio_trajectory_get_reference(&db->control_heading.trajectory, time_now, &dif_ref, &dif_ref_ext, &dif_rate_ref, &dif_acceleration_ref);
-    pbio_rate_integrator_get_errors(&db->control_heading.rate_integrator, time_now, dif_rate_ref, dif, dif_ref, &dif_rate_err, &dif_rate_err_integral);
-    buf[11] = dif_ref;
-    buf[12] = dif_rate_err_integral;
-    buf[13] = dif_rate_ref;
-    buf[14] = dif_rate_err_integral;
-
-    return pbio_logger_update(&db->log, buf);
-}
-
 pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, pbio_servo_t *left, pbio_servo_t *right, fix16_t wheel_diameter, fix16_t axle_track) {
     pbio_error_t err;
 
@@ -214,9 +174,6 @@ pbio_error_t pbio_drivebase_setup(pbio_drivebase_t *db, pbio_servo_t *left, pbio
     db->right = right;
     pbio_drivebase_claim_servos(db, false);
 
-    // Initialize log
-    db->log.num_values = DRIVEBASE_LOG_NUM_VALUES;
-
     // Adopt settings as the average or sum of both servos, except scaling
     err = drivebase_adopt_settings(&db->control_distance.settings, &db->control_heading.settings, &db->left->control.settings, &db->right->control.settings);
     if (err != PBIO_SUCCESS) {
@@ -310,18 +267,19 @@ pbio_error_t pbio_drivebase_stop_force(pbio_drivebase_t *db) {
 }
 
 pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
+
+    // If passive, log and exit
+    if (db->control_heading.type == PBIO_CONTROL_NONE || db->control_distance.type == PBIO_CONTROL_NONE) {
+        return PBIO_SUCCESS;
+    }
+
     // Get the physical state
     int32_t time_now, sum, sum_rate, dif, dif_rate;
     pbio_error_t err = drivebase_get_state(db, &time_now, &sum, &sum_rate, &dif, &dif_rate);
     if (err != PBIO_SUCCESS) {
         return err;
     }
 
-    // If passive, log and exit
-    if (db->control_heading.type == PBIO_CONTROL_NONE || db->control_distance.type == PBIO_CONTROL_NONE) {
-        return drivebase_log_update(db, time_now, sum, sum_rate, 0, dif, dif_rate, 0);
-    }
-
     // Get control signals
     int32_t sum_control, dif_control;
     pbio_actuation_t sum_actuation, dif_actuation;
@@ -334,11 +292,7 @@ pbio_error_t pbio_drivebase_update(pbio_drivebase_t *db) {
     }
 
     // Actuate
-    err = pbio_drivebase_actuate(db, sum_actuation, sum_control, dif_control);
-    if (err != PBIO_SUCCESS) {
-        return err;
-    }
-    return drivebase_log_update(db, time_now, sum, sum_rate, sum_control, dif, dif_rate, dif_control);
+    return pbio_drivebase_actuate(db, sum_actuation, sum_control, dif_control);
 }
 
 pbio_error_t pbio_drivebase_straight(pbio_drivebase_t *db, int32_t distance, int32_t drive_speed, int32_t drive_acceleration) {

lib/pbio/src/servo.c

@@ -11,12 +11,9 @@
 #include <pbdrv/motor.h>
 #include <pbio/math.h>
 #include <pbio/servo.h>
-#include <pbio/logger.h>
 
 #if PBDRV_CONFIG_NUM_MOTOR_CONTROLLER != 0
 
-#define SERVO_LOG_NUM_VALUES (9 + NUM_DEFAULT_LOG_VALUES)
-
 // TODO: Move to config and enable only known motors for platform
 static pbio_control_settings_t settings_servo_ev3_medium = {
     .max_rate = 2000,
@@ -184,9 +181,6 @@ pbio_error_t pbio_servo_setup(pbio_servo_t *srv, pbio_direction_t direction, fix
     // For a servo, counts per output unit is counts per degree at the gear train output
     srv->control.settings.counts_per_unit = fix16_mul(F16C(PBDRV_CONFIG_COUNTER_COUNTS_PER_DEGREE, 0), gear_ratio);
 
-    // Configure the logs for a servo
-    srv->log.num_values = SERVO_LOG_NUM_VALUES;
-
     return PBIO_SUCCESS;
 }
 
@@ -270,48 +264,6 @@ static pbio_error_t pbio_servo_actuate(pbio_servo_t *srv, pbio_actuation_t actua
     return PBIO_SUCCESS;
 }
 
-// Log motor data for a motor that is being actively controlled
-static pbio_error_t pbio_servo_log_update(pbio_servo_t *srv, int32_t time_now, int32_t count_now, int32_t rate_now, pbio_actuation_t actuation, int32_t control) {
-
-    int32_t buf[SERVO_LOG_NUM_VALUES];
-    memset(buf, 0, sizeof(buf));
-
-    // Log the physical state of the motor
-    buf[1] = count_now;
-    buf[2] = rate_now;
-
-    // Log the applied control signal
-    buf[3] = actuation;
-    buf[4] = control;
-
-    // If control is active, log additional data about the maneuver
-    if (srv->control.type != PBIO_CONTROL_NONE) {
-
-        // Get the time of reference evaluation
-        int32_t time_ref = pbio_control_get_ref_time(&srv->control, time_now);
-
-        // Log the time since start of control trajectory
-        buf[0] = (time_ref - srv->control.trajectory.t0) / 1000;
-
-        // Log reference signals. These values are only meaningful for time based commands
-        int32_t count_ref, count_ref_ext, rate_ref, err, err_integral, acceleration_ref;
-        pbio_trajectory_get_reference(&srv->control.trajectory, time_ref, &count_ref, &count_ref_ext, &rate_ref, &acceleration_ref);
-
-        if (srv->control.type == PBIO_CONTROL_ANGLE) {
-            pbio_count_integrator_get_errors(&srv->control.count_integrator, count_now, count_ref, &err, &err_integral);
-        } else {
-            pbio_rate_integrator_get_errors(&srv->control.rate_integrator, rate_now, rate_ref, count_now, count_ref, &err, &err_integral);
-        }
-
-        buf[5] = count_ref;
-        buf[6] = rate_ref;
-        buf[7] = err; // count err for angle control, rate err for timed control
-        buf[8] = err_integral;
-    }
-
-    return pbio_logger_update(&srv->log, buf);
-}
-
 pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) {
 
     // Read the physical state
@@ -335,20 +287,15 @@ pbio_error_t pbio_servo_control_update(pbio_servo_t *srv) {
         if (err != PBIO_SUCCESS) {
             return err;
         }
-        return pbio_servo_log_update(srv, time_now, count_now, rate_now, state, control);
+        // Log control data
+        int32_t log_data[] = {time_now, count_now, rate_now, state, control};
+        return pbio_logger_update(&srv->control.log, log_data);
     }
-
     // Calculate control signal
     control_update(&srv->control, time_now, count_now, rate_now, &actuation, &control);
 
     // Apply the control type and signal
-    err = pbio_servo_actuate(srv, actuation, control);
-    if (err != PBIO_SUCCESS) {
-        return err;
-    }
-
-    // Log data if logger enabled
-    return pbio_servo_log_update(srv, time_now, count_now, rate_now, actuation, control);
+    return pbio_servo_actuate(srv, actuation, control);
 }
 
 /* pbio user functions */

lib/pbio/test/src/servo.c

@@ -89,12 +89,12 @@ static PT_THREAD(test_servo_run_func(struct pt *pt, const char *name, pbio_error
     tt_uint_op(pbio_motorpoll_set_servo_status(servo, PBIO_ERROR_AGAIN), ==, PBIO_SUCCESS);
 
     // only logging one row since we read it after every iteration
-    log_buf = malloc(sizeof(*log_buf) * pbio_logger_cols(&servo->log));
+    log_buf = malloc(sizeof(*log_buf) * pbio_logger_cols(&servo->control.log));
     if (log_buf == NULL) {
         tt_abort_perror("failed to allocate log_buf");
     }
 
-    pbio_logger_start(&servo->log, log_buf, 1, 1);
+    pbio_logger_start(&servo->control.log, log_buf, 1, 1);
 
     tt_uint_op(func(servo), ==, PBIO_SUCCESS);
 
@@ -253,17 +253,17 @@ static PT_THREAD(test_servo_run_func(struct pt *pt, const char *name, pbio_error
             }
 
             // write current state to log file
-            if (pbio_logger_rows(&servo->log)) {
+            if (pbio_logger_rows(&servo->control.log)) {
                 fprintf(log_file, "%d,", clock_to_msec(clock_time()));
                 fprintf(log_file, "%d,", test_motor_driver.output);
                 fprintf(log_file, "%d,", test_motor_driver.duty_cycle);
 
-                for (int i = 0; i < pbio_logger_cols(&servo->log); i++) {
+                for (int i = 0; i < pbio_logger_cols(&servo->control.log); i++) {
                     fprintf(log_file, "%d,", log_buf[i]);
                 }
 
                 fprintf(log_file, "\n");
-                servo->log.sampled = 0;
+                servo->control.log.sampled = 0;
             }
 
             clock_tick(1);

pybricks/common.h

@@ -54,7 +54,6 @@ mp_obj_t logger_obj_make_new(pbio_log_t *log);
 typedef struct _common_Motor_obj_t {
     mp_obj_base_t base;
     pbio_servo_t *srv;
-    mp_obj_t logger;
     mp_obj_t control;
 } common_Motor_obj_t;
 

pybricks/common/pb_type_control.c

@@ -20,6 +20,7 @@ typedef struct _common_Control_obj_t {
     mp_obj_base_t base;
     pbio_control_t *control;
     mp_obj_t scale;
+    mp_obj_t logger;
 } common_Control_obj_t;
 
 // pybricks._common.Control.__init__/__new__
@@ -30,6 +31,10 @@ mp_obj_t common_Control_obj_make_new(pbio_control_t *control) {
 
     self->control = control;
 
+    // Create an instance of the Logger class
+    self->logger = logger_obj_make_new(&self->control->log);
+    self->control->log.num_values = MAX_LOG_VALUES;
+
     #if MICROPY_PY_BUILTINS_FLOAT
     self->scale = mp_obj_new_float(fix16_to_float(control->settings.counts_per_unit));
     #else
@@ -246,6 +251,7 @@ STATIC const mp_rom_map_elem_t common_Control_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_done), MP_ROM_PTR(&common_Control_done_obj) },
     { MP_ROM_QSTR(MP_QSTR_stalled), MP_ROM_PTR(&common_Control_stalled_obj) },
     { MP_ROM_QSTR(MP_QSTR_scale), MP_ROM_ATTRIBUTE_OFFSET(common_Control_obj_t, scale) },
+    { MP_ROM_QSTR(MP_QSTR_log), MP_ROM_ATTRIBUTE_OFFSET(common_Control_obj_t, logger) },
 };
 STATIC MP_DEFINE_CONST_DICT(common_Control_locals_dict, common_Control_locals_dict_table);
 

pybricks/common/pb_type_motor.c

@@ -92,9 +92,6 @@ STATIC mp_obj_t common_Motor_make_new(const mp_obj_type_t *type, size_t n_args,
     self->base.type = (mp_obj_type_t *)type;
     self->srv = srv;
 
-    // Create an instance of the Logger class
-    self->logger = logger_obj_make_new(&self->srv->log);
-
     // Create an instance of the Control class
     self->control = common_Control_obj_make_new(&self->srv->control);
 
@@ -336,7 +333,6 @@ STATIC const mp_rom_map_elem_t common_Motor_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_run_angle), MP_ROM_PTR(&common_Motor_run_angle_obj) },
     { MP_ROM_QSTR(MP_QSTR_run_target), MP_ROM_PTR(&common_Motor_run_target_obj) },
     { MP_ROM_QSTR(MP_QSTR_track_target), MP_ROM_PTR(&common_Motor_track_target_obj) },
-    { MP_ROM_QSTR(MP_QSTR_log), MP_ROM_ATTRIBUTE_OFFSET(common_Motor_obj_t, logger) },
     { MP_ROM_QSTR(MP_QSTR_control), MP_ROM_ATTRIBUTE_OFFSET(common_Motor_obj_t, control) },
 };
 MP_DEFINE_CONST_DICT(common_Motor_locals_dict, common_Motor_locals_dict_table);

pybricks/robotics/pb_type_drivebase.c

@@ -26,7 +26,6 @@ typedef struct _robotics_DriveBase_obj_t {
     pbio_drivebase_t *db;
     mp_obj_t left;
     mp_obj_t right;
-    mp_obj_t logger;
     mp_obj_t heading_control;
     mp_obj_t distance_control;
     int32_t straight_speed;
@@ -64,9 +63,6 @@ STATIC mp_obj_t robotics_DriveBase_make_new(const mp_obj_type_t *type, size_t n_
     pb_assert(pbio_drivebase_setup(self->db, srv_left, srv_right, pb_obj_get_fix16(wheel_diameter_in), pb_obj_get_fix16(axle_track_in)));
     pb_assert(pbio_motorpoll_set_drivebase_status(self->db, PBIO_ERROR_AGAIN));
 
-    // Create an instance of the Logger class
-    self->logger = logger_obj_make_new(&self->db->log);
-
     // Create instances of the Control class
     self->heading_control = common_Control_obj_make_new(&self->db->control_heading);
     self->distance_control = common_Control_obj_make_new(&self->db->control_distance);
@@ -254,7 +250,6 @@ STATIC const mp_rom_map_elem_t robotics_DriveBase_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_settings),         MP_ROM_PTR(&robotics_DriveBase_settings_obj) },
     { MP_ROM_QSTR(MP_QSTR_left),             MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, left)            },
     { MP_ROM_QSTR(MP_QSTR_right),            MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, right)           },
-    { MP_ROM_QSTR(MP_QSTR_log),              MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, logger)          },
     { MP_ROM_QSTR(MP_QSTR_heading_control),  MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, heading_control) },
     { MP_ROM_QSTR(MP_QSTR_distance_control), MP_ROM_ATTRIBUTE_OFFSET(robotics_DriveBase_obj_t, distance_control)},
 };