Browse Source

HAL_PX4: delay peripheral starup

moved to AP_BoardConfig
mission-4.1.18
Andrew Tridgell 8 years ago
parent
commit
84f93439ca
  1. 5
      libraries/AP_HAL_PX4/AnalogIn.cpp
  2. 12
      libraries/AP_HAL_PX4/AnalogIn.h
  3. 18
      libraries/AP_HAL_PX4/GPIO.h
  4. 5
      libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
  5. 18
      libraries/AP_HAL_PX4/RCInput.h

5
libraries/AP_HAL_PX4/AnalogIn.cpp

@ -246,6 +246,11 @@ void PX4AnalogIn::next_stop_pin(void) @@ -246,6 +246,11 @@ void PX4AnalogIn::next_stop_pin(void)
*/
void PX4AnalogIn::_timer_tick(void)
{
if (_adc_fd == -1) {
// not initialised yet
return;
}
// read adc at 100Hz
uint32_t now = AP_HAL::micros();
uint32_t delta_t = now - _last_run;

12
libraries/AP_HAL_PX4/AnalogIn.h

@ -52,15 +52,15 @@ private: @@ -52,15 +52,15 @@ private:
class PX4::PX4AnalogIn : public AP_HAL::AnalogIn {
public:
PX4AnalogIn();
void init();
AP_HAL::AnalogSource* channel(int16_t pin);
void init() override;
AP_HAL::AnalogSource* channel(int16_t pin) override;
void _timer_tick(void);
float board_voltage(void) { return _board_voltage; }
float servorail_voltage(void) { return _servorail_voltage; }
uint16_t power_status_flags(void) { return _power_flags; }
float board_voltage(void) override { return _board_voltage; }
float servorail_voltage(void) override { return _servorail_voltage; }
uint16_t power_status_flags(void) override { return _power_flags; }
private:
int _adc_fd;
int _adc_fd = -1;
int _battery_handle;
int _servorail_handle;
int _system_power_handle;

18
libraries/AP_HAL_PX4/GPIO.h

@ -27,21 +27,21 @@ @@ -27,21 +27,21 @@
class PX4::PX4GPIO : public AP_HAL::GPIO {
public:
PX4GPIO();
void init();
void pinMode(uint8_t pin, uint8_t output);
int8_t analogPinToDigitalPin(uint8_t pin);
uint8_t read(uint8_t pin);
void write(uint8_t pin, uint8_t value);
void toggle(uint8_t pin);
void init() override;
void pinMode(uint8_t pin, uint8_t output) override;
int8_t analogPinToDigitalPin(uint8_t pin) override;
uint8_t read(uint8_t pin) override;
void write(uint8_t pin, uint8_t value) override;
void toggle(uint8_t pin) override;
/* Alternative interface: */
AP_HAL::DigitalSource* channel(uint16_t n);
AP_HAL::DigitalSource* channel(uint16_t n) override;
/* Interrupt interface: */
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode);
bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) override;
/* return true if USB cable is connected */
bool usb_connected(void);
bool usb_connected(void) override;
// used by UART code to avoid a hw bug in the AUAV-X2
void set_usb_connected(void) { _usb_connected = true; }

5
libraries/AP_HAL_PX4/HAL_PX4_Class.cpp

@ -134,11 +134,6 @@ static int main_loop(int argc, char **argv) @@ -134,11 +134,6 @@ static int main_loop(int argc, char **argv)
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init();
hal.rcin->init();
hal.rcout->init();
hal.analogin->init();
hal.gpio->init();
/*
run setup() at low priority to ensure CLI doesn't hang the

18
libraries/AP_HAL_PX4/RCInput.h

@ -12,19 +12,19 @@ @@ -12,19 +12,19 @@
class PX4::PX4RCInput : public AP_HAL::RCInput {
public:
void init();
bool new_input();
uint8_t num_channels();
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
void init() override;
bool new_input() override;
uint8_t num_channels() override;
uint16_t read(uint8_t ch) override;
uint8_t read(uint16_t* periods, uint8_t len) override;
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override);
void clear_overrides();
bool set_overrides(int16_t *overrides, uint8_t len) override;
bool set_override(uint8_t channel, int16_t override) override;
void clear_overrides() override;
void _timer_tick(void);
bool rc_bind(int dsmMode);
bool rc_bind(int dsmMode) override;
private:
/* override state */

Loading…
Cancel
Save