pybricks.common.Motor: Implement load().

This replaces Control.load(). Now also works under passive conditions.

And it means it's back on Move Hub, which no longer has the Control
class.

Author: laurensvalk

CHANGELOG.md

@@ -5,7 +5,10 @@
 ## [Unreleased]
 
 ### Added
-- Added support for `PBIO_PYBRICKS_COMMAND_REBOOT_TO_UPDATE_MODE` Pybricks Profile BLE command.
+- Added support for `PBIO_PYBRICKS_COMMAND_REBOOT_TO_UPDATE_MODE` Pybricks
+  Profile BLE command.
+- Implemented `Motor.load()` which now measures load both during active
+  conditions (`run`) and passive conditions (`dc`).
 
 ### Changed
 - The Pybricks Code stop button will force the program to exit even if the user

lib/pbio/include/pbio/observer.h

@@ -93,6 +93,7 @@ void pbio_observer_reset(pbio_observer_t *obs, pbio_control_settings_t *settings
 void pbio_observer_get_estimated_state(const pbio_observer_t *obs, int32_t *speed_num, pbio_angle_t *angle_est, int32_t *speed_est);
 void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_t *angle, pbio_dcmotor_actuation_t actuation, int32_t voltage);
 bool pbio_observer_is_stalled(const pbio_observer_t *obs, uint32_t time, uint32_t *stall_duration);
+int32_t pbio_observer_get_feedback_torque(pbio_observer_t *obs, const pbio_angle_t *angle);
 
 // Model conversion functions:
 

lib/pbio/include/pbio/servo.h

@@ -93,6 +93,7 @@ pbio_error_t pbio_servo_get_state_control(pbio_servo_t *srv, pbio_control_state_
 pbio_error_t pbio_servo_get_state_user(pbio_servo_t *srv, int32_t *angle, int32_t *speed);
 bool pbio_servo_update_loop_is_running(pbio_servo_t *srv);
 pbio_error_t pbio_servo_is_stalled(pbio_servo_t *srv, bool *stalled, uint32_t *stall_duration);
+pbio_error_t pbio_servo_get_load(pbio_servo_t *srv, int32_t *load);
 
 // Servo end user commands:
 

lib/pbio/src/observer.c

@@ -105,6 +105,18 @@ static void update_stall_state(pbio_observer_t *obs, uint32_t time, pbio_dcmotor
     }
 }
 
+/**
+ * Gets observer feedback torque to keep it close to measured value.
+ *
+ * @param [in]  obs            The observer instance.
+ * @param [in]  angle          Measured angle used to correct the model.
+ * @return                     Feedback torque in uNm.
+ */
+int32_t pbio_observer_get_feedback_torque(pbio_observer_t *obs, const pbio_angle_t *angle) {
+    int32_t error = pbio_angle_diff_mdeg(angle, &obs->angle);
+    return pbio_control_settings_mul_by_gain(error, obs->model->gain);
+}
+
 /**
  * Predicts next system state and corrects the model using a measurement.
  *
@@ -125,10 +137,10 @@ void pbio_observer_update(pbio_observer_t *obs, uint32_t time, const pbio_angle_
     // Update numerical derivative as speed sanity check.
     obs->speed_numeric = pbio_differentiator_get_speed(&obs->differentiator, angle);
 
-    int32_t error = pbio_angle_diff_mdeg(angle, &obs->angle);
-
     // Apply observer error feedback as voltage.
-    int32_t feedback_voltage = pbio_observer_torque_to_voltage(m, pbio_control_settings_mul_by_gain(error, m->gain));
+    int32_t feedback_voltage = pbio_observer_torque_to_voltage(m,
+        pbio_observer_get_feedback_torque(obs, angle)
+        );
 
     // Check stall condition.
     update_stall_state(obs, time, actuation, voltage, feedback_voltage);

lib/pbio/src/servo.c

@@ -682,4 +682,49 @@ pbio_error_t pbio_servo_is_stalled(pbio_servo_t *srv, bool *stalled, uint32_t *s
     return PBIO_SUCCESS;
 }
 
+/**
+ * Gets estimated external load experienced by the servo.
+ *
+ * @param [in]  srv     The servo instance.
+ * @param [out] load    Estimated load (mNm).
+ * @return              Error code.
+ */
+pbio_error_t pbio_servo_get_load(pbio_servo_t *srv, int32_t *load) {
+
+    // Don't allow access if update loop not registered.
+    if (!pbio_servo_update_loop_is_running(srv)) {
+        *load = 0;
+        return PBIO_ERROR_INVALID_OP;
+    }
+
+    // Get passive actuation type.
+    pbio_dcmotor_actuation_t applied_actuation;
+    int32_t voltage;
+    pbio_dcmotor_get_state(srv->dcmotor, &applied_actuation, &voltage);
+
+    // Get best estimate based on control and physyical state.
+    if (applied_actuation == PBIO_DCMOTOR_ACTUATION_COAST) {
+        // Can't estimate load on coast.
+        *load = 0;
+    } else if (pbio_control_is_active(&srv->control)) {
+        // The experienced load is the opposite sign of what the PID is
+        // trying to overcome.
+        *load = -srv->control.pid_average;
+    } else {
+        // Read the angle.
+        pbio_angle_t angle;
+        pbio_error_t err = pbio_tacho_get_angle(srv->tacho, &angle);
+        if (err != PBIO_SUCCESS) {
+            return err;
+        }
+        // Use observer error as a measure of torque.
+        *load = pbio_observer_get_feedback_torque(&srv->observer, &angle);
+    }
+
+    // Convert to user torque units (mNm).
+    *load = pbio_control_settings_actuation_ctl_to_app(*load);
+
+    return PBIO_SUCCESS;
+}
+
 #endif // PBDRV_CONFIG_NUM_MOTOR_CONTROLLER

pybricks/common/pb_type_motor.c

@@ -362,6 +362,15 @@ STATIC mp_obj_t common_Motor_done(mp_obj_t self_in) {
 }
 MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_done_obj, common_Motor_done);
 
+// pybricks._common.Motor.load
+STATIC mp_obj_t common_Motor_load(mp_obj_t self_in) {
+    common_Motor_obj_t *self = MP_OBJ_TO_PTR(self_in);
+    int32_t load;
+    pb_assert(pbio_servo_get_load(self->srv, &load));
+    return mp_obj_new_int(load);
+}
+MP_DEFINE_CONST_FUN_OBJ_1(common_Motor_load_obj, common_Motor_load);
+
 // dir(pybricks.builtins.Motor)
 STATIC const mp_rom_map_elem_t common_Motor_locals_dict_table[] = {
     //
@@ -386,6 +395,7 @@ STATIC const mp_rom_map_elem_t common_Motor_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_stalled), MP_ROM_PTR(&common_Motor_stalled_obj) },
     { MP_ROM_QSTR(MP_QSTR_done), MP_ROM_PTR(&common_Motor_done_obj) },
     { MP_ROM_QSTR(MP_QSTR_track_target), MP_ROM_PTR(&common_Motor_track_target_obj) },
+    { MP_ROM_QSTR(MP_QSTR_load), MP_ROM_PTR(&common_Motor_load_obj) },
 };
 MP_DEFINE_CONST_DICT(common_Motor_locals_dict, common_Motor_locals_dict_table);