Browse Source

AP_HAL_SITL: GPIO minor fixes

fix style
make constructor explicit
fix implicit cast for unsigned to signed value
correct DigitalSource constructor definition
master
Pierre Kancir 8 years ago committed by Francisco Ferreira
parent
commit
7f5b32f59f
  1. 36
      libraries/AP_HAL_SITL/GPIO.cpp
  2. 10
      libraries/AP_HAL_SITL/GPIO.h

36
libraries/AP_HAL_SITL/GPIO.cpp

@ -16,30 +16,29 @@ int8_t GPIO::analogPinToDigitalPin(uint8_t pin)
return pin; return pin;
} }
uint8_t GPIO::read(uint8_t pin) uint8_t GPIO::read(uint8_t pin)
{ {
if (!sitlState->_sitl) { if (!_sitlState->_sitl) {
return 0; return 0;
} }
uint8_t mask = sitlState->_sitl->pin_mask.get(); uint8_t mask = static_cast<uint8_t>(_sitlState->_sitl->pin_mask.get());
return mask & (1U<<pin)? 1: 0; return static_cast<uint8_t>((mask & (1U << pin)) ? 1 : 0);
} }
void GPIO::write(uint8_t pin, uint8_t value) void GPIO::write(uint8_t pin, uint8_t value)
{ {
if (!sitlState->_sitl) { if (!_sitlState->_sitl) {
return; return;
} }
uint8_t mask = sitlState->_sitl->pin_mask.get(); uint8_t mask = static_cast<uint8_t>(_sitlState->_sitl->pin_mask.get());
uint8_t new_mask = mask; uint8_t new_mask = mask;
if (value) { if (value) {
new_mask |= (1U<<pin); new_mask |= (1U << pin);
} else { } else {
new_mask &= ~(1U<<pin); new_mask &= ~(1U << pin);
} }
if (mask != new_mask) { if (mask != new_mask) {
sitlState->_sitl->pin_mask.set_and_notify(new_mask); _sitlState->_sitl->pin_mask.set_and_notify(new_mask);
} }
} }
@ -50,7 +49,12 @@ void GPIO::toggle(uint8_t pin)
/* Alternative interface: */ /* Alternative interface: */
AP_HAL::DigitalSource* GPIO::channel(uint16_t n) { AP_HAL::DigitalSource* GPIO::channel(uint16_t n) {
return new DigitalSource(0); if (n < 8) { // (ie. sizeof(pin_mask)*8)
return new DigitalSource(static_cast<uint8_t>(n));
} else {
return nullptr;
}
} }
/* Interrupt interface: */ /* Interrupt interface: */
@ -64,8 +68,8 @@ bool GPIO::usb_connected(void)
return false; return false;
} }
DigitalSource::DigitalSource(uint8_t v) : DigitalSource::DigitalSource(uint8_t pin) :
pin(v) _pin(pin)
{} {}
void DigitalSource::mode(uint8_t output) void DigitalSource::mode(uint8_t output)
@ -73,16 +77,16 @@ void DigitalSource::mode(uint8_t output)
uint8_t DigitalSource::read() uint8_t DigitalSource::read()
{ {
return hal.gpio->read(pin); return hal.gpio->read(_pin);
} }
void DigitalSource::write(uint8_t value) void DigitalSource::write(uint8_t value)
{ {
value = value?1:0; value = static_cast<uint8_t>(value ? 1 : 0);
return hal.gpio->write(pin, value); return hal.gpio->write(_pin, value);
} }
void DigitalSource::toggle() void DigitalSource::toggle()
{ {
return hal.gpio->write(pin, !hal.gpio->read(pin)); return hal.gpio->write(_pin, !hal.gpio->read(_pin));
} }

10
libraries/AP_HAL_SITL/GPIO.h

@ -4,9 +4,7 @@
class HALSITL::GPIO : public AP_HAL::GPIO { class HALSITL::GPIO : public AP_HAL::GPIO {
public: public:
GPIO(SITL_State *_sitlState) { explicit GPIO(SITL_State *sitlState): _sitlState(sitlState) {}
sitlState = _sitlState;
}
void init(); void init();
void pinMode(uint8_t pin, uint8_t output); void pinMode(uint8_t pin, uint8_t output);
int8_t analogPinToDigitalPin(uint8_t pin); int8_t analogPinToDigitalPin(uint8_t pin);
@ -25,17 +23,17 @@ public:
bool usb_connected(void); bool usb_connected(void);
private: private:
SITL_State *sitlState; SITL_State *_sitlState;
}; };
class HALSITL::DigitalSource : public AP_HAL::DigitalSource { class HALSITL::DigitalSource : public AP_HAL::DigitalSource {
public: public:
DigitalSource(uint8_t _pin); explicit DigitalSource(uint8_t pin);
void mode(uint8_t output); void mode(uint8_t output);
uint8_t read(); uint8_t read();
void write(uint8_t value); void write(uint8_t value);
void toggle(); void toggle();
private: private:
uint8_t pin; uint8_t _pin;
}; };

Loading…
Cancel
Save