5
0
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:
Andrew Tridgell 2018-08-01 18:56:50 +10:00 committed by Randy Mackay
parent d1a16a0e2f
commit 252ab82c97
6 changed files with 81 additions and 18 deletions

View File

@ -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) {

View File

@ -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;

View File

@ -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"

View File

@ -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, &param);
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

View File

@ -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

View File

@ -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;
}