aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_PX4: support FMUv4

Browse Source
This commit is contained in:
Andrew Tridgell 2015-11-25 13:06:43 +11:00
parent 8739c55d27
commit 9835043dd5
5 changed files with 38 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -48,7 +48,7 @@ static const struct {
{ 11, 6.6f/4096 }, // analog airspeed input, 2:1 scaling { 11, 6.6f/4096 }, // analog airspeed input, 2:1 scaling
{ 12, 3.3f/4096 }, // analog2, on SPI port pin 3 { 12, 3.3f/4096 }, // analog2, on SPI port pin 3
{ 13, 16.8f/4096 }, // analog3, on SPI port pin 4 { 13, 16.8f/4096 }, // analog3, on SPI port pin 4
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
{ 2, 3.3f/4096 }, // 3DR Brick voltage, usually 10.1:1 { 2, 3.3f/4096 }, // 3DR Brick voltage, usually 10.1:1
// scaled from battery voltage // scaled from battery voltage
{ 3, 3.3f/4096 }, // 3DR Brick current, usually 17:1 scaled { 3, 3.3f/4096 }, // 3DR Brick current, usually 17:1 scaled
@ -269,7 +269,7 @@ void PX4AnalogIn::_timer_tick(void)
if (ret > 0) { if (ret > 0) {
// match the incoming channels to the currently active pins // match the incoming channels to the currently active pins
for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) { for (uint8_t i=0; i<ret/sizeof(buf_adc[0]); i++) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
if (buf_adc[i].am_channel == 4) { if (buf_adc[i].am_channel == 4) {
// record the Vcc value for later use in // record the Vcc value for later use in
// voltage_average_ratiometric() // voltage_average_ratiometric()
@ -325,7 +325,7 @@ void PX4AnalogIn::_timer_tick(void)
} }
#endif #endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
// check for new servorail data on FMUv2 // check for new servorail data on FMUv2
if (_servorail_handle != -1) { if (_servorail_handle != -1) {
struct servorail_status_s servorail; struct servorail_status_s servorail;

2
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -14,7 +14,7 @@
// these are virtual pins that read from the ORB // these are virtual pins that read from the ORB
#define PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN 100 #define PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN 100
#define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN 101 #define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN 101
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
#define PX4_ANALOG_VCC_5V_PIN 4 #define PX4_ANALOG_VCC_5V_PIN 4
#define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN 102 #define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN 102
#define PX4_ANALOG_ORB_SERVO_VRSSI_PIN 103 #define PX4_ANALOG_ORB_SERVO_VRSSI_PIN 103

33
libraries/AP_HAL_PX4/GPIO.cpp
View File

@ -58,11 +58,13 @@ void PX4GPIO::init()
} }
#endif #endif
#ifdef PX4IO_DEVICE_PATH
// also try to setup for the relay pins on the IO board // also try to setup for the relay pins on the IO board
_gpio_io_fd = open(PX4IO_DEVICE_PATH, O_RDWR); _gpio_io_fd = open(PX4IO_DEVICE_PATH, O_RDWR);
if (_gpio_io_fd == -1) { if (_gpio_io_fd == -1) {
hal.console->printf("GPIO: Unable to open px4io\n"); hal.console->printf("GPIO: Unable to open px4io\n");
} }
#endif
} }
void PX4GPIO::pinMode(uint8_t pin, uint8_t output) void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
@ -117,6 +119,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
#ifdef PX4IO_P_SETUP_RELAYS_POWER1 #ifdef PX4IO_P_SETUP_RELAYS_POWER1
case PX4_GPIO_EXT_IO_RELAY1_PIN: { case PX4_GPIO_EXT_IO_RELAY1_PIN: {
uint32_t relays = 0; uint32_t relays = 0;
if (_gpio_io_fd == -1) {
return LOW;
}
ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays); ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
return (relays & PX4IO_P_SETUP_RELAYS_POWER1)?HIGH:LOW; return (relays & PX4IO_P_SETUP_RELAYS_POWER1)?HIGH:LOW;
} }
@ -125,6 +130,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
#ifdef PX4IO_P_SETUP_RELAYS_POWER2 #ifdef PX4IO_P_SETUP_RELAYS_POWER2
case PX4_GPIO_EXT_IO_RELAY2_PIN: { case PX4_GPIO_EXT_IO_RELAY2_PIN: {
uint32_t relays = 0; uint32_t relays = 0;
if (_gpio_io_fd == -1) {
return LOW;
}
ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays); ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
return (relays & PX4IO_P_SETUP_RELAYS_POWER2)?HIGH:LOW; return (relays & PX4IO_P_SETUP_RELAYS_POWER2)?HIGH:LOW;
} }
@ -133,6 +141,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
#ifdef PX4IO_P_SETUP_RELAYS_ACC1 #ifdef PX4IO_P_SETUP_RELAYS_ACC1
case PX4_GPIO_EXT_IO_ACC1_PIN: { case PX4_GPIO_EXT_IO_ACC1_PIN: {
uint32_t relays = 0; uint32_t relays = 0;
if (_gpio_io_fd == -1) {
return LOW;
}
ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays); ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
return (relays & PX4IO_P_SETUP_RELAYS_ACC1)?HIGH:LOW; return (relays & PX4IO_P_SETUP_RELAYS_ACC1)?HIGH:LOW;
} }
@ -141,6 +152,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
#ifdef PX4IO_P_SETUP_RELAYS_ACC2 #ifdef PX4IO_P_SETUP_RELAYS_ACC2
case PX4_GPIO_EXT_IO_ACC2_PIN: { case PX4_GPIO_EXT_IO_ACC2_PIN: {
uint32_t relays = 0; uint32_t relays = 0;
if (_gpio_io_fd == -1) {
return LOW;
}
ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays); ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
return (relays & PX4IO_P_SETUP_RELAYS_ACC2)?HIGH:LOW; return (relays & PX4IO_P_SETUP_RELAYS_ACC2)?HIGH:LOW;
} }
@ -148,6 +162,9 @@ uint8_t PX4GPIO::read(uint8_t pin) {
case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5): { case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5): {
uint32_t relays = 0; uint32_t relays = 0;
if (_gpio_io_fd == -1) {
return LOW;
}
ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays); ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
return (relays & (1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0))))?HIGH:LOW; return (relays & (1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0))))?HIGH:LOW;
} }
@ -203,25 +220,33 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
#ifdef PX4IO_P_SETUP_RELAYS_POWER1 #ifdef PX4IO_P_SETUP_RELAYS_POWER1
case PX4_GPIO_EXT_IO_RELAY1_PIN: case PX4_GPIO_EXT_IO_RELAY1_PIN:
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER1); if (_gpio_io_fd != -1) {
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER1);
}
break; break;
#endif #endif
#ifdef PX4IO_P_SETUP_RELAYS_POWER2 #ifdef PX4IO_P_SETUP_RELAYS_POWER2
case PX4_GPIO_EXT_IO_RELAY2_PIN: case PX4_GPIO_EXT_IO_RELAY2_PIN:
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER2); if (_gpio_io_fd != -1) {
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_POWER2);
}
break; break;
#endif #endif
#ifdef PX4IO_P_SETUP_RELAYS_ACC1 #ifdef PX4IO_P_SETUP_RELAYS_ACC1
case PX4_GPIO_EXT_IO_ACC1_PIN: case PX4_GPIO_EXT_IO_ACC1_PIN:
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC1); if (_gpio_io_fd != -1) {
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC1);
}
break; break;
#endif #endif
#ifdef PX4IO_P_SETUP_RELAYS_ACC2 #ifdef PX4IO_P_SETUP_RELAYS_ACC2
case PX4_GPIO_EXT_IO_ACC2_PIN: case PX4_GPIO_EXT_IO_ACC2_PIN:
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC2); if (_gpio_io_fd != -1) {
ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC2);
}
break; break;
#endif #endif

8
libraries/AP_HAL_PX4/GPIO.h
View File

@ -50,10 +50,10 @@ public:
void set_usb_connected(void) { _usb_connected = true; } void set_usb_connected(void) { _usb_connected = true; }
private: private:
int _led_fd; int _led_fd = -1;
int _tone_alarm_fd; int _tone_alarm_fd = -1;
int _gpio_fmu_fd; int _gpio_fmu_fd = -1;
int _gpio_io_fd; int _gpio_io_fd = -1;
bool _usb_connected = false; bool _usb_connected = false;
}; };

2
libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
View File

@ -43,7 +43,7 @@ static PX4AnalogIn analogIn;
static PX4Util utilInstance; static PX4Util utilInstance;
static PX4GPIO gpioDriver; static PX4GPIO gpioDriver;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS3" #define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS1" #define UARTC_DEFAULT_DEVICE "/dev/ttyS1"