Secure STM32F1 servo code and M280 detach (#14085)

This commit is contained in:
Tanguy Pruvot 2019-05-22 23:57:19 +02:00 committed by Scott Lahteine
parent a49919396a
commit 24b1818245
3 changed files with 16 additions and 9 deletions

View file

@ -27,7 +27,7 @@
#if HAS_SERVOS #if HAS_SERVOS
uint8_t ServoCount; //=0 uint8_t ServoCount = 0;
#include "HAL_Servo_STM32F1.h" #include "HAL_Servo_STM32F1.h"
@ -68,16 +68,16 @@ libServo::libServo() {
bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) { bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t maxAngle) {
if (this->servoIndex >= MAX_SERVOS) return false; if (this->servoIndex >= MAX_SERVOS) return false;
if (!PWM_PIN(pin)) return false;
this->pin = pin;
this->minAngle = minAngle; this->minAngle = minAngle;
this->maxAngle = maxAngle; this->maxAngle = maxAngle;
timer_dev *tdev = PIN_MAP[this->pin].timer_device; timer_dev *tdev = PIN_MAP[pin].timer_device;
uint8_t tchan = PIN_MAP[this->pin].timer_channel; uint8_t tchan = PIN_MAP[pin].timer_channel;
pinMode(this->pin, PWM); pinMode(pin, PWM);
pwmWrite(this->pin, 0); pwmWrite(pin, 0);
timer_pause(tdev); timer_pause(tdev);
timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based timer_set_prescaler(tdev, SERVO_PRESCALER - 1); // prescaler is 1-based
@ -85,6 +85,8 @@ bool libServo::attach(const int32_t pin, const int32_t minAngle, const int32_t m
timer_generate_update(tdev); timer_generate_update(tdev);
timer_resume(tdev); timer_resume(tdev);
this->pin = pin; // set attached()
return true; return true;
} }

View file

@ -51,7 +51,7 @@
#define IS_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD) #define IS_INPUT(IO) (_GET_MODE(IO) == GPIO_INPUT_FLOATING || _GET_MODE(IO) == GPIO_INPUT_ANALOG || _GET_MODE(IO) == GPIO_INPUT_PU || _GET_MODE(IO) == GPIO_INPUT_PD)
#define IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP) #define IS_OUTPUT(IO) (_GET_MODE(IO) == GPIO_OUTPUT_PP)
#define PWM_PIN(P) (PIN_MAP[IO].timer_device != nullptr) #define PWM_PIN(IO) (PIN_MAP[IO].timer_device != nullptr)
// digitalRead/Write wrappers // digitalRead/Write wrappers
#define extDigitalRead(IO) digitalRead(IO) #define extDigitalRead(IO) digitalRead(IO)

View file

@ -34,8 +34,13 @@ void GcodeSuite::M280() {
if (!parser.seen('P')) return; if (!parser.seen('P')) return;
const int servo_index = parser.value_int(); const int servo_index = parser.value_int();
if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) { if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) {
if (parser.seen('S')) if (parser.seen('S')) {
MOVE_SERVO(servo_index, parser.value_int()); const int a = parser.value_int();
if (a == -1)
servo[servo_index].detach();
else
MOVE_SERVO(servo_index, a);
}
else { else {
SERIAL_ECHO_START(); SERIAL_ECHO_START();
SERIAL_ECHOPAIR(" Servo ", servo_index); SERIAL_ECHOPAIR(" Servo ", servo_index);