diff --git a/lib/pbio/drv/ioport/ioport_lpf2.c b/lib/pbio/drv/ioport/ioport_lpf2.c index dad80df2d..b00041ee9 100644 --- a/lib/pbio/drv/ioport/ioport_lpf2.c +++ b/lib/pbio/drv/ioport/ioport_lpf2.c @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -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 index a2707e2c0..97e70e5f0 100644 --- a/lib/pbio/include/pbdrv/ioport.h +++ b/lib/pbio/include/pbdrv/ioport.h @@ -20,6 +20,8 @@ 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) { @@ -27,6 +29,9 @@ static inline pbio_error_t pbdrv_ioport_get_iodev(pbio_port_t port, pbio_iodev_t 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 index d4fcd8d78..2e487172b 100644 --- a/lib/pbio/src/main.c +++ b/lib/pbio/src/main.c @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #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(); }