Handle motor stalls and fix run_to_position properly for real

Author: RMJ10

include/device.h

@@ -95,5 +95,10 @@ extern int device_push_mode(PyObject *self, int mode);
 /* Restore the previously pushed mode */
 extern int device_pop_mode(PyObject *self);
 
+/* Ensures that the initialisation that cannot be done in the background
+ * has in fact been done.
+ */
+extern int device_ensure_mode_info(PyObject *self);
+
 
 #endif /* RPI_STRAWBERRY_DEVICE_H_INCLUDED */

include/motor-settings.h

@@ -34,10 +34,6 @@
 #define MOTOR_STOP_HOLD  2
 #define MOTOR_STOP_USE_DEFAULT 3
 
-#define DIRECTION_CLOCKWISE     0
-#define DIRECTION_ANTICLOCKWISE 1
-#define DIRECTION_SHORTEST      2
-
 /* Values passed to the cmd functions */
 #define STOP_FLOAT 0
 #define STOP_HOLD 126

include/motor.h

@@ -19,6 +19,7 @@
 #define PY_SSIZE_T_CLEAN
 #include <Python.h>
 
+#include <stdint.h>
 #include "device.h"
 
 extern int motor_modinit(void);
@@ -42,9 +43,24 @@ extern void motor_detach(PyObject *self);
      (dt) == ID_STONE_GREY_MOTOR_MEDIUM ||     \
      (dt) == ID_STONE_GREY_MOTOR_LARGE)
 
-/* Read the position of the motor */
-extern int motor_get_position(PyObject *self,
-                              long *ppos_from_zero,
-                              long *ppos_from_preset);
+/* Read the position of the motor relative to the preset mark */
+extern int motor_get_position(PyObject *self, long *pposition);
+
+/* Set the "preset" marker position relative to zero */
+extern int motor_set_preset(PyObject *self, long position);
+
+/* Updates the "preset" marker (for MotorPair.preset()) */
+extern void motor_update_preset(PyObject *self, long position);
+
+/* Ensure foregrounded initialisation is done (for MotorPairs) */
+extern int motor_ensure_mode_info(PyObject *self);
+
+/* Get the offset to use when determining absolute motor positions */
+/* NB: the mode information must have been acquired at some point
+ * before calling this.  This is taken care of at init time for
+ * MotorPairs, but if you call this function from any other situation
+ * you should first call motor_ensure_mode_info()
+ */
+extern int32_t motor_get_position_offset(PyObject *self);
 
 #endif /* RPI_STRAWBERRY_MOTOR_H_INCLUDED */

include/port.h

@@ -73,6 +73,7 @@ extern PyObject *port_get_device(PyObject *port);
 extern PyObject *ports_get_value_dict(PyObject *portset);
 extern PyObject *port_get_motor(PyObject *port);  /* Returns new reference */
 extern int port_is_motor(PyObject *port); /* True if a motor is connected */
+extern int port_set_motor_preset(PyObject *port, long position);
 
 /* The following are called only from the callback thread */
 extern int port_handle_callback(uint8_t port_id, uint8_t event);

src/cmd.c

@@ -625,33 +625,36 @@ static int wait_for_complete_feedback(uint8_t port_id, uint8_t *response)
                 free(buffer);
                 return -1;
             }
-            if (buffer[0] != 5 ||
-                buffer[2] != TYPE_PORT_OUTPUT_FEEDBACK ||
-                buffer[3] != port_id)
-            {
-                free(buffer);
-                PyErr_SetString(hub_protocol_error,
-                                "Unexpected reply to Output command");
-                return -1;
-            }
             if (buffer[2] == TYPE_GENERIC_ERROR)
             {
                 handle_generic_error(TYPE_PORT_OUTPUT, buffer);
                 free(buffer);
                 return -1;
             }
-            if ((buffer[4] & 0x04) != 0)
+            if (buffer[0] == 5 &&
+                buffer[2] == TYPE_PORT_OUTPUT_FEEDBACK &&
+                buffer[3] == port_id)
             {
-                /* "Current Command(s) Discarded" bit set */
-                free(buffer);
-                PyErr_SetString(hub_protocol_error, "Port busy");
-                return -1;
-            }
-            if ((buffer[4] & 0x02) != 0)
-            {
-                /* "Current command(s) Complete" bit set */
-                free(buffer);
-                return 0;
+                if ((buffer[4] & 0x20) != 0)
+                {
+                    /* The motor has stalled! */
+                    free(buffer);
+                    PyErr_SetString(hub_protocol_error, "Motor stalled");
+                    return -1;
+                }
+                if ((buffer[4] & 0x04) != 0)
+                {
+                    /* "Current Command(s) Discarded" bit set */
+                    free(buffer);
+                    PyErr_SetString(hub_protocol_error, "Port busy");
+                    return -1;
+                }
+                if ((buffer[4] & 0x02) != 0)
+                {
+                    /* "Current command(s) Complete" bit set */
+                    free(buffer);
+                    return 0;
+                }
             }
             free(buffer);
         }

src/device.c

@@ -21,6 +21,7 @@
 
 #include "device.h"
 #include "port.h"
+#include "motor.h"
 
 /**
 
@@ -258,6 +259,11 @@ static default_mode_t default_modes[NUM_DEFAULT_MODES] = {
 #undef MODE
 
 
+/* Forward declarations */
+static int get_value(DeviceObject *self);
+static int extract_value(DeviceObject *self, uint8_t index, long *pvalue);
+
+
 static default_mode_t *
 get_default_mode(uint16_t id)
 {
@@ -526,21 +532,40 @@ get_mode_info(DeviceObject *device, uint8_t port_id)
                  default_mode->mode_mask) == default_mode->mode_mask)
             {
                 /* This has all the modes we need */
-                return set_combi_mode(device, combi_index,
-                                      default_mode->mode_list,
-                                      default_mode->num_modes);
+                if (set_combi_mode(device, combi_index,
+                                   default_mode->mode_list,
+                                   default_mode->num_modes) < 0)
+                    return -1;
+                break;
             }
         }
-        /* We can't find a suitable combi-mode.  Oh well */
+
+        /* Assert that any formally recognised motor is set up to Get
+         * the Speed, Position, Absolute Position and Power, and needs
+         * to have its delta (preset) calculated.
+         */
+        if (motor_is_motor(device->type_id))
+        {
+            long position_from_zero;
+
+            if (get_value(device) < 0)
+                return -1;
+            if (extract_value(device, 2, &position_from_zero) < 0)
+                return -1;
+            if (port_set_motor_preset(device->port, position_from_zero) < 0)
+                return -1;
+        }
     }
 
     return 0;
 }
 
 
-static int
-ensure_mode_info(DeviceObject *device)
+int
+device_ensure_mode_info(PyObject *self)
 {
+    DeviceObject *device = (DeviceObject *)self;
+
     if ((device->flags & DO_FLAGS_GOT_MODE_INFO) == 0)
         return get_mode_info(device, port_get_id(device->port));
 
@@ -562,7 +587,7 @@ Device_mode(PyObject *self, PyObject *args)
     }
     if (!PyArg_ParseTuple(args, "|OO:mode", &arg1, &arg2))
         return NULL;
-    if (ensure_mode_info(device) < 0)
+    if (device_ensure_mode_info(device) < 0)
         return NULL;
 
     if (arg1 == NULL)
@@ -946,7 +971,7 @@ Device_get(PyObject *self, PyObject *args)
         }
     }
 
-    if (ensure_mode_info(device) < 0)
+    if (device_ensure_mode_info(device) < 0)
         return NULL;
     if (!device->is_unreported)
     {
@@ -1530,7 +1555,7 @@ int device_is_in_mode(PyObject *self, int mode)
     DeviceObject *device = (DeviceObject *)self;
     uint8_t i;
 
-    if (ensure_mode_info(device) < 0)
+    if (device_ensure_mode_info(device) < 0)
         return -1;
     if (device->current_mode == mode)
         return 1;
@@ -1608,7 +1633,7 @@ int device_push_mode(PyObject *self, int mode)
 {
     DeviceObject *device = (DeviceObject *)self;
 
-    if (ensure_mode_info(device) < 0)
+    if (device_ensure_mode_info(device) < 0)
         return -1;
 
     if (mode < 0 || mode >= device->num_modes)
@@ -1638,7 +1663,7 @@ int device_pop_mode(PyObject *self)
 {
     DeviceObject *device = (DeviceObject *)self;
 
-    if (ensure_mode_info(device) < 0)
+    if (device_ensure_mode_info(device) < 0)
         return -1;
 
     if (device->saved_current_mode == MODE_IS_COMBI)