mirror of https://github.com/ArduPilot/ardupilot
147 lines
3.6 KiB
C++
147 lines
3.6 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include <AP_HAL.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|
|
#include <AP_HAL_PX4.h>
|
|
#include "AP_HAL_PX4_Namespace.h"
|
|
#include "HAL_PX4_Class.h"
|
|
#include "Console.h"
|
|
#include "Scheduler.h"
|
|
#include "UARTDriver.h"
|
|
#include "Storage.h"
|
|
#include "RCInput.h"
|
|
#include "RCOutput.h"
|
|
|
|
#include <AP_HAL_Empty.h>
|
|
#include <AP_HAL_Empty_Private.h>
|
|
|
|
#include <stdlib.h>
|
|
#include <poll.h>
|
|
#include <systemlib/systemlib.h>
|
|
#include <nuttx/config.h>
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
|
|
using namespace PX4;
|
|
|
|
static Empty::EmptySemaphore i2cSemaphore;
|
|
static Empty::EmptyI2CDriver i2cDriver(&i2cSemaphore);
|
|
static Empty::EmptySPIDeviceManager spiDeviceManager;
|
|
static Empty::EmptyAnalogIn analogIn;
|
|
static Empty::EmptyGPIO gpioDriver;
|
|
static Empty::EmptyUtil utilInstance;
|
|
|
|
static PX4ConsoleDriver consoleDriver;
|
|
static PX4Scheduler schedulerInstance;
|
|
static PX4EEPROMStorage storageDriver;
|
|
static PX4RCInput rcinDriver;
|
|
static PX4RCOutput rcoutDriver;
|
|
|
|
#define UARTA_DEVICE "/dev/ttyS2"
|
|
#define UARTB_DEVICE "/dev/ttyS3"
|
|
|
|
// only two real UART drivers for now
|
|
static PX4UARTDriver uartADriver(UARTA_DEVICE);
|
|
static PX4UARTDriver uartBDriver(UARTB_DEVICE);
|
|
static Empty::EmptyUARTDriver uartCDriver;
|
|
|
|
HAL_PX4::HAL_PX4() :
|
|
AP_HAL::HAL(
|
|
&uartADriver, /* uartA */
|
|
&uartBDriver, /* uartB */
|
|
&uartCDriver, /* uartC */
|
|
&i2cDriver, /* i2c */
|
|
&spiDeviceManager, /* spi */
|
|
&analogIn, /* analogin */
|
|
&storageDriver, /* storage */
|
|
&consoleDriver, /* console */
|
|
&gpioDriver, /* gpio */
|
|
&rcinDriver, /* rcinput */
|
|
&rcoutDriver, /* rcoutput */
|
|
&schedulerInstance, /* scheduler */
|
|
&utilInstance) /* util */
|
|
{}
|
|
|
|
static bool thread_should_exit = false; /**< Daemon exit flag */
|
|
static bool thread_running = false; /**< Daemon status flag */
|
|
static int daemon_task; /**< Handle of daemon task / thread */
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
static int main_loop(int argc, char **argv)
|
|
{
|
|
extern void setup(void);
|
|
extern void loop(void);
|
|
|
|
hal.uartA->begin(115200);
|
|
hal.console->init((void*) hal.uartA);
|
|
hal.scheduler->init(NULL);
|
|
hal.rcin->init(NULL);
|
|
hal.rcout->init(NULL);
|
|
|
|
setup();
|
|
hal.scheduler->system_initialized();
|
|
|
|
while (true) {
|
|
loop();
|
|
|
|
// yield the CPU for 1ms between loops to let other apps
|
|
// get some CPU time
|
|
poll(NULL, 0, 1);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
void HAL_PX4::init(int argc, char * const argv[]) const
|
|
{
|
|
if (argc < 1) {
|
|
printf("%s: missing command (try '%s start')",
|
|
SKETCHNAME, SKETCHNAME);
|
|
exit(1);
|
|
}
|
|
|
|
if (strcmp(argv[1], "start") == 0) {
|
|
if (thread_running) {
|
|
printf("%s already running\n", SKETCHNAME);
|
|
/* this is not an error */
|
|
exit(0);
|
|
}
|
|
|
|
printf("Starting %s on %s\n", SKETCHNAME, UARTA_DEVICE);
|
|
|
|
thread_should_exit = false;
|
|
daemon_task = task_spawn(SKETCHNAME,
|
|
SCHED_RR,
|
|
SCHED_PRIORITY_DEFAULT,
|
|
4096,
|
|
main_loop,
|
|
NULL);
|
|
exit(0);
|
|
}
|
|
|
|
if (strcmp(argv[1], "stop") == 0) {
|
|
thread_should_exit = true;
|
|
exit(0);
|
|
}
|
|
|
|
if (strcmp(argv[1], "status") == 0) {
|
|
if (thread_running) {
|
|
printf("\t%s is running\n", SKETCHNAME);
|
|
} else {
|
|
printf("\t%s is not started\n", SKETCHNAME);
|
|
}
|
|
exit(0);
|
|
}
|
|
|
|
printf("%s: unrecognized command ('start', 'stop' or 'status')", SKETCHNAME);
|
|
exit(1);
|
|
}
|
|
|
|
const HAL_PX4 AP_HAL_PX4;
|
|
|
|
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
|
|