mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-14 12:48:31 -04:00
HAL_PX4: update to match plane3.9.0beta6
This commit is contained in:
parent
d1a16a0e2f
commit
252ab82c97
libraries/AP_HAL_PX4
@ -91,17 +91,6 @@ void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
|
||||
}
|
||||
}
|
||||
|
||||
int8_t PX4GPIO::analogPinToDigitalPin(uint8_t pin)
|
||||
{
|
||||
switch (pin) {
|
||||
case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
|
||||
// the only pins that can be mapped are the FMU servo rail pins */
|
||||
return pin;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
uint8_t PX4GPIO::read(uint8_t pin) {
|
||||
switch (pin) {
|
||||
|
||||
|
@ -37,7 +37,6 @@ public:
|
||||
PX4GPIO();
|
||||
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;
|
||||
|
@ -76,6 +76,13 @@ static PX4::SPIDeviceManager spi_mgr_instance;
|
||||
#define UARTD_DEFAULT_DEVICE "/dev/null"
|
||||
#define UARTE_DEFAULT_DEVICE "/dev/null"
|
||||
#define UARTF_DEFAULT_DEVICE "/dev/null"
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
|
||||
#define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
|
||||
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
|
||||
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
|
||||
#define UARTE_DEFAULT_DEVICE "/dev/null"
|
||||
#define UARTF_DEFAULT_DEVICE "/dev/null"
|
||||
#else
|
||||
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
|
||||
#define UARTB_DEFAULT_DEVICE "/dev/ttyS3"
|
||||
|
@ -172,17 +172,13 @@ void PX4Scheduler::delay_microseconds_boost(uint16_t usec)
|
||||
|
||||
void PX4Scheduler::delay(uint16_t ms)
|
||||
{
|
||||
if (!in_main_thread()) {
|
||||
::printf("ERROR: delay() from timer process\n");
|
||||
return;
|
||||
}
|
||||
perf_begin(_perf_delay);
|
||||
uint64_t start = AP_HAL::micros64();
|
||||
|
||||
while ((AP_HAL::micros64() - start)/1000 < ms &&
|
||||
!_px4_thread_should_exit) {
|
||||
delay_microseconds_semaphore(1000);
|
||||
if (_min_delay_cb_ms <= ms) {
|
||||
if (in_main_thread() && _min_delay_cb_ms <= ms) {
|
||||
call_delay_cb();
|
||||
}
|
||||
}
|
||||
@ -446,4 +442,68 @@ void PX4Scheduler::restore_interrupts(void *state)
|
||||
irqrestore((irqstate_t)(uintptr_t)state);
|
||||
}
|
||||
|
||||
/*
|
||||
trampoline for thread create
|
||||
*/
|
||||
void *PX4Scheduler::thread_create_trampoline(void *ctx)
|
||||
{
|
||||
AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
|
||||
(*t)();
|
||||
free(t);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
/*
|
||||
create a new thread
|
||||
*/
|
||||
bool PX4Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
|
||||
{
|
||||
// take a copy of the MemberProc, it is freed after thread exits
|
||||
AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
|
||||
if (!tproc) {
|
||||
return false;
|
||||
}
|
||||
*tproc = proc;
|
||||
|
||||
uint8_t thread_priority = APM_IO_PRIORITY;
|
||||
static const struct {
|
||||
priority_base base;
|
||||
uint8_t p;
|
||||
} priority_map[] = {
|
||||
{ PRIORITY_BOOST, APM_MAIN_PRIORITY_BOOST},
|
||||
{ PRIORITY_MAIN, APM_MAIN_PRIORITY},
|
||||
{ PRIORITY_SPI, APM_SPI_PRIORITY},
|
||||
{ PRIORITY_I2C, APM_I2C_PRIORITY},
|
||||
{ PRIORITY_CAN, APM_CAN_PRIORITY},
|
||||
{ PRIORITY_TIMER, APM_TIMER_PRIORITY},
|
||||
{ PRIORITY_RCIN, APM_TIMER_PRIORITY},
|
||||
{ PRIORITY_IO, APM_IO_PRIORITY},
|
||||
{ PRIORITY_UART, APM_UART_PRIORITY},
|
||||
{ PRIORITY_STORAGE, APM_STORAGE_PRIORITY},
|
||||
};
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(priority_map); i++) {
|
||||
if (priority_map[i].base == base) {
|
||||
thread_priority = constrain_int16(priority_map[i].p + priority, 1, APM_MAX_PRIORITY);
|
||||
break;
|
||||
}
|
||||
}
|
||||
pthread_t thread;
|
||||
pthread_attr_t thread_attr;
|
||||
struct sched_param param;
|
||||
|
||||
pthread_attr_init(&thread_attr);
|
||||
pthread_attr_setstacksize(&thread_attr, stack_size);
|
||||
|
||||
param.sched_priority = thread_priority;
|
||||
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
||||
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
||||
|
||||
if (pthread_create(&thread, &thread_attr, thread_create_trampoline, tproc) != 0) {
|
||||
free(tproc);
|
||||
return false;
|
||||
}
|
||||
pthread_setname_np(thread, name);
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
@ -10,6 +10,7 @@
|
||||
|
||||
#define PX4_SCHEDULER_MAX_TIMER_PROCS 8
|
||||
|
||||
#define APM_MAX_PRIORITY 243
|
||||
#define APM_MAIN_PRIORITY_BOOST 241
|
||||
#define APM_MAIN_PRIORITY 180
|
||||
#define APM_TIMER_PRIORITY 181
|
||||
@ -72,6 +73,11 @@ public:
|
||||
restore interrupt state from disable_interrupts_save()
|
||||
*/
|
||||
void restore_interrupts(void *) override;
|
||||
|
||||
/*
|
||||
create a new thread
|
||||
*/
|
||||
bool thread_create(AP_HAL::MemberProc, const char *name, uint32_t stack_size, priority_base base, int8_t priority) override;
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
@ -115,5 +121,6 @@ private:
|
||||
perf_counter_t _perf_io_timers;
|
||||
perf_counter_t _perf_storage_timer;
|
||||
perf_counter_t _perf_delay;
|
||||
static void *thread_create_trampoline(void *ctx);
|
||||
};
|
||||
#endif
|
||||
|
@ -126,7 +126,8 @@ bool PX4Util::get_system_id(char buf[40])
|
||||
board_type,
|
||||
(unsigned)serialid[0], (unsigned)serialid[1], (unsigned)serialid[2], (unsigned)serialid[3],
|
||||
(unsigned)serialid[4], (unsigned)serialid[5], (unsigned)serialid[6], (unsigned)serialid[7],
|
||||
(unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
|
||||
(unsigned)serialid[8], (unsigned)serialid[9], (unsigned)serialid[10],(unsigned)serialid[11]);
|
||||
buf[39] = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user