mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_PX4: implement new tonealarm interface
This commit is contained in:
parent
f4fc9249f7
commit
4e48923c64
|
@ -15,6 +15,7 @@
|
|||
#include <drivers/drv_gpio.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
@ -278,5 +279,28 @@ bool PX4Util::flash_bootloader()
|
|||
return false;
|
||||
}
|
||||
|
||||
bool PX4Util::toneAlarm_init() {
|
||||
_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) {
|
||||
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
|
||||
|
|
|
@ -61,6 +61,9 @@ public:
|
|||
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
|
||||
|
||||
bool flash_bootloader() override;
|
||||
|
||||
bool toneAlarm_init() override;
|
||||
void toneAlarm_set_buzzer_tone(float frequency, float volume) override;
|
||||
|
||||
private:
|
||||
int _safety_handle;
|
||||
|
@ -74,4 +77,6 @@ private:
|
|||
uint32_t last_update_ms;
|
||||
int fd = -1;
|
||||
} _heater;
|
||||
|
||||
int _tonealarm_fd;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue