pbdrv/ioport: turn off passive devices on stop

This ensures that devices like train motors stop when the program ends. Fixes pybricks/support#226
pybricks · Jan 22, 2021 · 7fadcb3 · 7fadcb3
1 parent b22ad15
commit 7fadcb3
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 0 deletions.
diff --git a/lib/pbio/drv/ioport/ioport_lpf2.c b/lib/pbio/drv/ioport/ioport_lpf2.c
@@ -15,6 +15,7 @@
 #include <lego_uart.h>
 
 #include <pbdrv/gpio.h>
+#include <pbdrv/motor.h>
 #include <pbio/error.h>
 #include <pbio/iodev.h>
 #include <pbio/uartdev.h>
@@ -254,6 +255,15 @@ pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) {
     return PBIO_SUCCESS;
 }
 
+// Turns off power to passive (non-uart) devices
+void pbio_ioport_reset_passive_devices(void) {
+    for (int i = 0; i < PBDRV_CONFIG_IOPORT_LPF2_NUM_PORTS; i++) {
+        if (ioport_devs[i].connected_type_id != PBIO_IODEV_TYPE_ID_LPF2_UNKNOWN_UART) {
+            pbdrv_motor_coast(i + PBDRV_CONFIG_FIRST_IO_PORT);
+        }
+    }
+}
+
 // This is the device connection manager (dcm). It monitors the ID1 and ID2 pins
 // on the port to see when devices are connected or disconnected.
 // It is expected for there to be a 2ms delay between calls to this function.

diff --git a/lib/pbio/include/pbdrv/ioport.h b/lib/pbio/include/pbdrv/ioport.h
@@ -20,13 +20,18 @@
 
 pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev);
 
+void pbio_ioport_reset_passive_devices(void);
+
 #else // PBDRV_CONFIG_IOPORT
 
 static inline pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t **iodev) {
     *iodev = NULL;
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 
+static inline void pbio_ioport_reset_passive_devices(void) {
+}
+
 #endif // PBDRV_CONFIG_IOPORT
 
 #endif // _PBDRV_IOPORT_H_

diff --git a/lib/pbio/src/main.c b/lib/pbio/src/main.c
@@ -16,6 +16,7 @@
 #include <pbdrv/motor.h>
 #include <pbdrv/sound.h>
 #include <pbio/config.h>
+#include <pbdrv/ioport.h>
 #include <pbio/light_matrix.h>
 #include <pbio/light.h>
 #include "pbio/motor_process.h"
@@ -71,6 +72,9 @@ void pbio_stop_all(void) {
     #if PBIO_CONFIG_LIGHT
     pbio_light_animation_stop_all();
     #endif
+    #if PBDRV_CONFIG_IOPORT_LPF2
+    pbio_ioport_reset_passive_devices();
+    #endif
     pbio_motor_process_reset();
     pbdrv_sound_stop();
 }