mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: updated to new tonealarm system
This commit is contained in:
parent
d9435261ce
commit
7f58c0a63d
|
@ -47,11 +47,6 @@ void PX4GPIO::init()
|
||||||
#endif
|
#endif
|
||||||
#endif
|
#endif
|
||||||
#if !defined(CONFIG_ARCH_BOARD_AEROFC_V1)
|
#if !defined(CONFIG_ARCH_BOARD_AEROFC_V1)
|
||||||
_tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
|
|
||||||
if (_tone_alarm_fd == -1) {
|
|
||||||
AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
|
|
||||||
}
|
|
||||||
|
|
||||||
_gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
|
_gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
|
||||||
if (_gpio_fmu_fd == -1) {
|
if (_gpio_fmu_fd == -1) {
|
||||||
AP_HAL::panic("Unable to open GPIO");
|
AP_HAL::panic("Unable to open GPIO");
|
||||||
|
@ -193,15 +188,6 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
case PX4_GPIO_PIEZO_PIN: // Piezo beeper
|
|
||||||
if (value == LOW) { // this is inverted
|
|
||||||
ioctl(_tone_alarm_fd, TONE_SET_ALARM, 3); // Alarm on !!
|
|
||||||
//::write(_tone_alarm_fd, &user_tune, sizeof(user_tune));
|
|
||||||
} else {
|
|
||||||
ioctl(_tone_alarm_fd, TONE_SET_ALARM, 0); // Alarm off !!
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
|
|
||||||
#ifdef GPIO_EXT_1
|
#ifdef GPIO_EXT_1
|
||||||
case PX4_GPIO_EXT_FMU_RELAY1_PIN:
|
case PX4_GPIO_EXT_FMU_RELAY1_PIN:
|
||||||
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_EXT_1);
|
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_EXT_1);
|
||||||
|
|
|
@ -55,7 +55,6 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int _led_fd = -1;
|
int _led_fd = -1;
|
||||||
int _tone_alarm_fd = -1;
|
|
||||||
int _gpio_fmu_fd = -1;
|
int _gpio_fmu_fd = -1;
|
||||||
int _gpio_io_fd = -1;
|
int _gpio_io_fd = -1;
|
||||||
bool _usb_connected = false;
|
bool _usb_connected = false;
|
||||||
|
|
|
@ -15,6 +15,7 @@
|
||||||
#include <drivers/drv_gpio.h>
|
#include <drivers/drv_gpio.h>
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||||
|
#include <drivers/drv_tone_alarm.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
@ -265,6 +266,7 @@ void PX4Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_ty
|
||||||
|
|
||||||
extern "C" {
|
extern "C" {
|
||||||
int bl_update_main(int argc, char *argv[]);
|
int bl_update_main(int argc, char *argv[]);
|
||||||
|
int tone_alarm_main(int argc, char *argv[]);
|
||||||
};
|
};
|
||||||
|
|
||||||
bool PX4Util::flash_bootloader()
|
bool PX4Util::flash_bootloader()
|
||||||
|
@ -278,5 +280,35 @@ bool PX4Util::flash_bootloader()
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool PX4Util::toneAlarm_init()
|
||||||
|
{
|
||||||
|
if (!AP_BoardConfig::px4_start_driver(tone_alarm_main, "tone_alarm", "start")) {
|
||||||
|
hal.console->printf("Failed to start tone_alarm\n");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
_tonealarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
|
||||||
|
if (_tonealarm_fd == -1) {
|
||||||
|
hal.console->printf("ToneAlarm_PX4: Unable to open " TONEALARM0_DEVICE_PATH);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void PX4Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
|
||||||
|
{
|
||||||
|
if (_tonealarm_fd == -1) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (is_zero(frequency) || is_zero(volume)) {
|
||||||
|
write(_tonealarm_fd, "MFP", 4);
|
||||||
|
} else {
|
||||||
|
unsigned note = constrain_float(roundf(17.3123404906676f*logf(frequency) - 71.3763165622959f), 4, 87); // constraint based on comment in PX4 tonealarm driver - "note value in the range C1 to B7"
|
||||||
|
char tune[20];
|
||||||
|
snprintf(tune, sizeof(tune), "MFMLT32L1N%u", note);
|
||||||
|
tune[sizeof(tune)-1] = '\0';
|
||||||
|
write(_tonealarm_fd, tune, strlen(tune)+1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||||
|
|
|
@ -61,6 +61,9 @@ public:
|
||||||
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
|
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
|
||||||
|
|
||||||
bool flash_bootloader() override;
|
bool flash_bootloader() override;
|
||||||
|
|
||||||
|
bool toneAlarm_init() override;
|
||||||
|
void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int _safety_handle;
|
int _safety_handle;
|
||||||
|
@ -74,4 +77,6 @@ private:
|
||||||
uint32_t last_update_ms;
|
uint32_t last_update_ms;
|
||||||
int fd = -1;
|
int fd = -1;
|
||||||
} _heater;
|
} _heater;
|
||||||
|
|
||||||
|
int _tonealarm_fd = -1;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue