Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

VRBRAIN: updated AP_HAL_VRBRAIN

Browse Source
This commit is contained in:
LukeMike 2016-07-05 16:15:55 +02:00 committed by Andrew Tridgell
parent 5255f55cc3
commit fde5992b6d
23 changed files with 3929 additions and 3010 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL_VRBRAIN/AP_HAL_VRBRAIN_Namespace.h
View File

@ -11,4 +11,8 @@ namespace VRBRAIN {
class VRBRAINUtil;
class VRBRAINGPIO;
class VRBRAINDigitalSource;
class NSHShellStream;
class VRBRAINI2CDriver;
class VRBRAIN_I2C;
class Semaphore;
}

44
libraries/AP_HAL_VRBRAIN/AnalogIn.cpp
View File

@ -18,14 +18,14 @@
#include <uORB/topics/system_power.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <errno.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include "GPIO.h"
#define ANLOGIN_DEBUGGING 0
// base voltage scaling for 12 bit 3.3V ADC
#define VRBRAIN_VOLTAGE_SCALING (3.3f/4096.0f)
#if ANALOGIN_DEBUGGING
#if ANLOGIN_DEBUGGING
# define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
# define Debug(fmt, args ...)
@ -41,8 +41,7 @@ static const struct {
uint8_t pin;
float scaling;
} pin_scaling[] = {
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
{ 0, 3.3f/4096 },
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
{ 10, 3.3f/4096 },
{ 11, 3.3f/4096 },
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
@ -52,11 +51,6 @@ static const struct {
{ 2, 3.3f/4096 },
{ 3, 3.3f/4096 },
{ 10, 3.3f/4096 },
#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10)
{ 10, 3.3f/4096 },
{ 11, 3.3f/4096 },
{ 14, 3.3f/4096 },
{ 15, 3.3f/4096 },
#else
#error "Unknown board type for AnalogIn scaling"
#endif
@ -75,6 +69,11 @@ VRBRAINAnalogSource::VRBRAINAnalogSource(int16_t pin, float initial_value) :
_sum_value(0),
_sum_ratiometric(0)
{
}
void VRBRAINAnalogSource::set_stop_pin(uint8_t p)
@ -192,9 +191,9 @@ VRBRAINAnalogIn::VRBRAINAnalogIn() :
void VRBRAINAnalogIn::init()
{
_adc_fd = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
_adc_fd = open(ADC0_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
if (_adc_fd == -1) {
AP_HAL::panic("Unable to open " ADC_DEVICE_PATH);
AP_HAL::panic("Unable to open " ADC0_DEVICE_PATH);
}
_battery_handle = orb_subscribe(ORB_ID(battery_status));
_servorail_handle = orb_subscribe(ORB_ID(servorail_status));
@ -282,29 +281,6 @@ void VRBRAINAnalogIn::_timer_tick(void)
}
}
// check for new battery data on FMUv1
if (_battery_handle != -1) {
struct battery_status_s battery;
bool updated = false;
if (orb_check(_battery_handle, &updated) == 0 && updated) {
orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
if (battery.timestamp != _battery_timestamp) {
_battery_timestamp = battery.timestamp;
for (uint8_t j=0; j<VRBRAIN_ANALOG_MAX_CHANNELS; j++) {
VRBRAIN::VRBRAINAnalogSource *c = _channels[j];
if (c == NULL) continue;
if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
c->_add_value(battery.voltage_v / VRBRAIN_VOLTAGE_SCALING, 0);
}
if (c->_pin == VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN) {
// scale it back to voltage, knowing that the
// px4io code scales by 90.0/5.0
c->_add_value(battery.current_a * (5.0f/90.0f) / VRBRAIN_VOLTAGE_SCALING, 0);
}
}
}
}
}
}
AP_HAL::AnalogSource* VRBRAINAnalogIn::channel(int16_t pin)

2
libraries/AP_HAL_VRBRAIN/AnalogIn.h
View File

@ -8,7 +8,7 @@
#define VRBRAIN_ANALOG_MAX_CHANNELS 16
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRHERO_V10)
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V51) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V52) || defined(CONFIG_ARCH_BOARD_VRCORE_V10) || defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
#define VRBRAIN_ANALOG_ORB_BATTERY_VOLTAGE_PIN 10
#define VRBRAIN_ANALOG_ORB_BATTERY_CURRENT_PIN 11
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)

170
libraries/AP_HAL_VRBRAIN/GPIO.cpp
View File

@ -13,7 +13,7 @@
/* VRBRAIN headers */
#include <drivers/drv_led.h>
#include <drivers/drv_buzzer.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_gpio.h>
#include <arch/board/board.h>
@ -31,9 +31,9 @@ VRBRAINGPIO::VRBRAINGPIO()
void VRBRAINGPIO::init()
{
_led_fd = open(LED_DEVICE_PATH, O_RDWR);
_led_fd = open(LED0_DEVICE_PATH, O_RDWR);
if (_led_fd == -1) {
AP_HAL::panic("Unable to open " LED_DEVICE_PATH);
AP_HAL::panic("Unable to open " LED0_DEVICE_PATH);
}
if (ioctl(_led_fd, LED_OFF, LED_BLUE) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED BLUE\n");
@ -44,38 +44,30 @@ void VRBRAINGPIO::init()
if (ioctl(_led_fd, LED_OFF, LED_GREEN) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED GREEN\n");
}
#if defined(LED_EXT1)
if (ioctl(_led_fd, LED_OFF, LED_EXT1) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 1\n");
}
#endif
#if defined(LED_EXT2)
if (ioctl(_led_fd, LED_OFF, LED_EXT2) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 2\n");
}
#endif
#if defined(LED_EXT3)
if (ioctl(_led_fd, LED_OFF, LED_EXT3) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO LED EXT 3\n");
}
#endif
#if defined(BUZZER_EXT)
_buzzer_fd = open(BUZZER_DEVICE_PATH, O_RDWR);
if (_buzzer_fd == -1) {
AP_HAL::panic("Unable to open " BUZZER_DEVICE_PATH);
_tone_alarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
if (_tone_alarm_fd == -1) {
AP_HAL::panic("Unable to open " TONEALARM0_DEVICE_PATH);
}
if (ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO BUZZER\n");
_gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
if (_gpio_fmu_fd == -1) {
AP_HAL::panic("Unable to open GPIO");
}
#ifdef GPIO_SERVO_1
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_1) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO_1\n");
}
#endif
#if defined(GPIO_GPIO0_OUTPUT)
stm32_configgpio(GPIO_GPIO0_OUTPUT);
#ifdef GPIO_SERVO_2
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_2) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO_2\n");
}
#endif
#if defined(GPIO_GPIO1_OUTPUT)
stm32_configgpio(GPIO_GPIO1_OUTPUT);
#ifdef GPIO_SERVO_3
if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_SERVO_3) != 0) {
hal.console->printf("GPIO: Unable to setup GPIO_3\n");
}
#endif
}
@ -91,20 +83,17 @@ int8_t VRBRAINGPIO::analogPinToDigitalPin(uint8_t pin)
}
uint8_t VRBRAINGPIO::read(uint8_t pin)
{
uint8_t VRBRAINGPIO::read(uint8_t pin) {
switch (pin) {
case EXTERNAL_RELAY1_PIN:
#if defined(GPIO_GPIO0_OUTPUT)
return (stm32_gpioread(GPIO_GPIO0_OUTPUT))?HIGH:LOW;
#ifdef GPIO_SERVO_3
case EXTERNAL_RELAY1_PIN: {
uint32_t relays = 0;
ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&relays);
return (relays & GPIO_SERVO_3)?HIGH:LOW;
}
#endif
break;
case EXTERNAL_RELAY2_PIN:
#if defined(GPIO_GPIO1_OUTPUT)
return (stm32_gpioread(GPIO_GPIO1_OUTPUT))?HIGH:LOW;
#endif
break;
}
return LOW;
}
@ -137,102 +126,31 @@ void VRBRAINGPIO::write(uint8_t pin, uint8_t value)
}
break;
#ifdef GPIO_SERVO_1
case EXTERNAL_LED_GPS:
#if defined(LED_EXT1)
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_EXT1);
} else {
ioctl(_led_fd, LED_ON, LED_EXT1);
}
#endif
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_1);
break;
#endif
#ifdef GPIO_SERVO_2
case EXTERNAL_LED_ARMED:
#if defined(LED_EXT2)
if (value == LOW) {
ioctl(_led_fd, LED_OFF, LED_EXT2);
} else {
ioctl(_led_fd, LED_ON, LED_EXT2);
}
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_2);
break;
#endif
break;
case EXTERNAL_LED_MOTOR1:
break;
case EXTERNAL_LED_MOTOR2:
break;
case BUZZER_PIN:
#if defined(BUZZER_EXT)
if (value == LOW) {
ioctl(_buzzer_fd, BUZZER_OFF, BUZZER_EXT);
} else {
ioctl(_buzzer_fd, BUZZER_ON, BUZZER_EXT);
}
#endif
break;
#ifdef GPIO_SERVO_3
case EXTERNAL_RELAY1_PIN:
#if defined(GPIO_GPIO0_OUTPUT)
stm32_gpiowrite(GPIO_GPIO0_OUTPUT, (value==HIGH));
#endif
ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, GPIO_SERVO_3);
break;
case EXTERNAL_RELAY2_PIN:
#if defined(GPIO_GPIO1_OUTPUT)
stm32_gpiowrite(GPIO_GPIO1_OUTPUT, (value==HIGH));
#endif
break;
}
}
void VRBRAINGPIO::toggle(uint8_t pin)
{
switch (pin) {
case HAL_GPIO_A_LED_PIN: // Arming LED
ioctl(_led_fd, LED_TOGGLE, LED_GREEN);
break;
case HAL_GPIO_B_LED_PIN: // not used yet
ioctl(_led_fd, LED_TOGGLE, LED_BLUE);
break;
case HAL_GPIO_C_LED_PIN: // GPS LED
ioctl(_led_fd, LED_TOGGLE, LED_RED);
break;
case EXTERNAL_LED_GPS:
#if defined(LED_EXT1)
ioctl(_led_fd, LED_TOGGLE, LED_EXT1);
#endif
break;
case EXTERNAL_LED_ARMED:
#if defined(LED_EXT2)
ioctl(_led_fd, LED_TOGGLE, LED_EXT2);
#endif
break;
case EXTERNAL_LED_MOTOR1:
break;
case EXTERNAL_LED_MOTOR2:
break;
case BUZZER_PIN:
#if defined(BUZZER_EXT)
ioctl(_buzzer_fd, BUZZER_TOGGLE, BUZZER_EXT);
#endif
break;
default:
write(pin, !read(pin));
break;
}
}
/* Alternative interface: */
@ -251,7 +169,13 @@ bool VRBRAINGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_
*/
bool VRBRAINGPIO::usb_connected(void)
{
return stm32_gpioread(GPIO_OTGFS_VBUS);
/*
we use a combination of voltage on the USB connector and the
open of the /dev/ttyACM0 character device. This copes with
systems where the VBUS may go high even with no USB connected
(such as AUAV-X2)
*/
return stm32_gpioread(GPIO_OTGFS_VBUS) && _usb_connected;
}

16
libraries/AP_HAL_VRBRAIN/GPIO.h
View File

@ -3,6 +3,7 @@
#include "AP_HAL_VRBRAIN.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define HAL_GPIO_A_LED_PIN 25
# define HAL_GPIO_B_LED_PIN 26
# define HAL_GPIO_C_LED_PIN 27
@ -10,11 +11,12 @@
# define EXTERNAL_LED_ARMED 29
# define EXTERNAL_LED_MOTOR1 30
# define EXTERNAL_LED_MOTOR2 31
# define BUZZER_PIN 32
# define EXTERNAL_RELAY1_PIN 33
# define EXTERNAL_RELAY2_PIN 34
# define HAL_GPIO_LED_ON HIGH
# define HAL_GPIO_LED_OFF LOW
#endif
class VRBRAIN::VRBRAINGPIO : public AP_HAL::GPIO {
public:
@ -35,9 +37,15 @@ public:
/* return true if USB cable is connected */
bool usb_connected(void);
// used by UART code to avoid a hw bug in the AUAV-X2
void set_usb_connected(void) { _usb_connected = true; }
private:
int _led_fd;
int _buzzer_fd;
int _led_fd = -1;
int _tone_alarm_fd = -1;
int _gpio_fmu_fd = -1;
bool _usb_connected = false;
};
class VRBRAIN::VRBRAINDigitalSource : public AP_HAL::DigitalSource {

75
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.cpp
View File

@ -4,8 +4,6 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <assert.h>
#include "AP_HAL_VRBRAIN.h"
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "HAL_VRBRAIN_Class.h"
@ -17,6 +15,7 @@
#include "AnalogIn.h"
#include "Util.h"
#include "GPIO.h"
#include "I2CDevice.h"
#include <AP_HAL_Empty/AP_HAL_Empty.h>
#include <AP_HAL_Empty/AP_HAL_Empty_Private.h>
@ -33,11 +32,8 @@
using namespace VRBRAIN;
static Empty::SPIDeviceManager spiDeviceManager;
static Empty::OpticalFlow optflowDriver;
//static Empty::GPIO gpioDriver;
static Empty::I2CDeviceManager i2c_mgr_instance;
static VRBRAINScheduler schedulerInstance;
static VRBRAINStorage storageDriver;
static VRBRAINRCInput rcinDriver;
@ -46,58 +42,73 @@ static VRBRAINAnalogIn analogIn;
static VRBRAINUtil utilInstance;
static VRBRAINGPIO gpioDriver;
//We only support 3 serials for VRBRAIN at the moment
static VRBRAIN::I2CDeviceManager i2c_mgr_instance;
#if defined(CONFIG_ARCH_BOARD_VRBRAIN_V45)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V51)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTE_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V52)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTE_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V51)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10)
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
#define UARTA_DEFAULT_DEVICE "/dev/ttyACM0"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#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/ttyS1"
#define UARTB_DEFAULT_DEVICE "/dev/ttyS0"
#define UARTC_DEFAULT_DEVICE "/dev/ttyS2"
#define UARTD_DEFAULT_DEVICE "/dev/null"
#define UARTD_DEFAULT_DEVICE "/dev/ttyS1"
#define UARTE_DEFAULT_DEVICE "/dev/null"
#define UARTF_DEFAULT_DEVICE "/dev/null"
#endif
// 3 UART drivers, for GPS plus two mavlink-enabled devices
static VRBRAINUARTDriver uartADriver(UARTA_DEFAULT_DEVICE, "APM_uartA");
static VRBRAINUARTDriver uartBDriver(UARTB_DEFAULT_DEVICE, "APM_uartB");
static VRBRAINUARTDriver uartCDriver(UARTC_DEFAULT_DEVICE, "APM_uartC");
static VRBRAINUARTDriver uartDDriver(UARTD_DEFAULT_DEVICE, "APM_uartD");
static VRBRAINUARTDriver uartEDriver(UARTE_DEFAULT_DEVICE, "APM_uartE");
static VRBRAINUARTDriver uartFDriver(UARTF_DEFAULT_DEVICE, "APM_uartF");
HAL_VRBRAIN::HAL_VRBRAIN() :
AP_HAL::HAL(
@ -106,7 +117,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
&uartCDriver, /* uartC */
&uartDDriver, /* uartD */
&uartEDriver, /* uartE */
NULL, // uartF
&uartFDriver, /* uartF */
&i2c_mgr_instance,
&spiDeviceManager, /* spi */
&analogIn, /* analogin */
@ -117,7 +128,7 @@ HAL_VRBRAIN::HAL_VRBRAIN() :
&rcoutDriver, /* rcoutput */
&schedulerInstance, /* scheduler */
&utilInstance, /* util */
&optflowDriver) /* optflow */
NULL) /* no onboard optical flow */
{}
bool _vrbrain_thread_should_exit = false; /**< Daemon exit flag */
@ -130,7 +141,7 @@ extern const AP_HAL::HAL& hal;
/*
set the priority of the main APM task
*/
static void set_priority(uint8_t priority)
void hal_vrbrain_set_priority(uint8_t priority)
{
struct sched_param param;
param.sched_priority = priority;
@ -145,16 +156,14 @@ static void set_priority(uint8_t priority)
*/
static void loop_overtime(void *)
{
set_priority(APM_OVERTIME_PRIORITY);
hal_vrbrain_set_priority(APM_OVERTIME_PRIORITY);
vrbrain_ran_overtime = true;
}
static AP_HAL::HAL::Callbacks* g_callbacks;
static int main_loop(int argc, char **argv)
{
extern void setup(void);
extern void loop(void);
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
@ -171,11 +180,11 @@ static int main_loop(int argc, char **argv)
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
*/
set_priority(APM_STARTUP_PRIORITY);
hal_vrbrain_set_priority(APM_STARTUP_PRIORITY);
schedulerInstance.hal_initialized();
setup();
g_callbacks->setup();
hal.scheduler->system_initialized();
perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
@ -187,7 +196,7 @@ static int main_loop(int argc, char **argv)
/*
switch to high priority for main loop
*/
set_priority(APM_MAIN_PRIORITY);
hal_vrbrain_set_priority(APM_MAIN_PRIORITY);
while (!_vrbrain_thread_should_exit) {
perf_begin(perf_loop);
@ -200,14 +209,14 @@ static int main_loop(int argc, char **argv)
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
loop();
g_callbacks->loop();
if (vrbrain_ran_overtime) {
/*
we ran over 1s in loop(), and our priority was lowered
to let a driver run. Set it back to high priority now.
*/
set_priority(APM_MAIN_PRIORITY);
hal_vrbrain_set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
vrbrain_ran_overtime = false;
}
@ -215,11 +224,11 @@ static int main_loop(int argc, char **argv)
perf_end(perf_loop);
/*
give up 500 microseconds of time, to ensure drivers get a
give up 250 microseconds of time, to ensure drivers get a
chance to run. This relies on the accurate semaphore wait
using hrt in semaphore.cpp
*/
hal.scheduler->delay_microseconds(500);
hal.scheduler->delay_microseconds(250);
}
thread_running = false;
return 0;
@ -271,10 +280,10 @@ void HAL_VRBRAIN::run(int argc, char * const argv[], Callbacks* callbacks) const
SKETCHNAME, deviceA, deviceC, deviceD, deviceE);
_vrbrain_thread_should_exit = false;
daemon_task = task_spawn_cmd(SKETCHNAME,
daemon_task = px4_task_spawn_cmd(SKETCHNAME,
SCHED_FIFO,
APM_MAIN_PRIORITY,
8192,
APM_MAIN_THREAD_STACK_SIZE,
main_loop,
NULL);
exit(0);

2
libraries/AP_HAL_VRBRAIN/HAL_VRBRAIN_Class.h
View File

@ -15,4 +15,6 @@ public:
void run(int argc, char* const argv[], Callbacks* callbacks) const override;
};
void hal_vrbrain_set_priority(uint8_t priority);
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

143
libraries/AP_HAL_VRBRAIN/NSHShellStream.cpp Normal file
View File

@ -0,0 +1,143 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
implementation of NSH shell as a stream, for use in nsh over MAVLink
See GCS_serial_control.cpp for usage
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <stdio.h>
#include <stdarg.h>
#include <unistd.h>
#include <stdlib.h>
#include <errno.h>
#include <apps/nsh.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include "Scheduler.h"
extern const AP_HAL::HAL& hal;
#include "Util.h"
using namespace VRBRAIN;
void NSHShellStream::shell_thread(void *arg)
{
NSHShellStream *nsh = (NSHShellStream *)arg;
close(0);
close(1);
close(2);
dup2(nsh->child.in, 0);
dup2(nsh->child.out, 1);
dup2(nsh->child.out, 2);
nsh_consolemain(0, NULL);
nsh->shell_stdin = -1;
nsh->shell_stdout = -1;
nsh->child.in = -1;
nsh->child.out = -1;
}
void NSHShellStream::start_shell(void)
{
if (hal.util->available_memory() < 8192) {
if (!showed_memory_warning) {
showed_memory_warning = true;
hal.console->printf("Not enough memory for shell\n");
}
return;
}
if (hal.util->get_soft_armed()) {
if (!showed_armed_warning) {
showed_armed_warning = true;
hal.console->printf("Disarm to start nsh shell\n");
}
// don't allow shell start when armed
return;
}
int p1[2], p2[2];
if (pipe(p1) != 0) {
return;
}
if (pipe(p2) != 0) {
return;
}
shell_stdin = p1[0];
shell_stdout = p2[1];
child.in = p2[0];
child.out = p1[1];
pthread_attr_t thread_attr;
struct sched_param param;
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 2048);
param.sched_priority = APM_SHELL_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&shell_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::NSHShellStream::shell_thread, this);
}
size_t NSHShellStream::write(uint8_t b)
{
if (shell_stdout == -1) {
start_shell();
}
if (shell_stdout != -1) {
return ::write(shell_stdout, &b, 1);
}
return 0;
}
size_t NSHShellStream::write(const uint8_t *buffer, size_t size)
{
size_t ret = 0;
while (size > 0) {
if (write(*buffer++) != 1) {
break;
}
ret++;
size--;
}
return ret;
}
int16_t NSHShellStream::read()
{
if (shell_stdin == -1) {
start_shell();
}
if (shell_stdin != -1) {
uint8_t b;
if (::read(shell_stdin, &b, 1) == 1) {
return b;
}
}
return -1;
}
uint32_t NSHShellStream::available()
{
uint32_t ret = 0;
if (ioctl(shell_stdin, FIONREAD, (unsigned long)&ret) == OK) {
return ret;
}
return 0;
}
uint32_t NSHShellStream::txspace()
{
uint32_t ret = 0;
if (ioctl(shell_stdout, FIONWRITE, (unsigned long)&ret) == OK) {
return ret;
}
return 0;
}
#endif // CONFIG_HAL_BOARD

21
libraries/AP_HAL_VRBRAIN/RCInput.cpp
View File

@ -2,6 +2,9 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "RCInput.h"
#include <fcntl.h>
#include <unistd.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
@ -114,4 +117,22 @@ void VRBRAINRCInput::_timer_tick(void)
perf_end(_perf_rcin);
}
bool VRBRAINRCInput::rc_bind(int dsmMode)
{
return true;
}
#endif

7
libraries/AP_HAL_VRBRAIN/RCInput.h
View File

@ -5,6 +5,11 @@
#include <systemlib/perf_counter.h>
#include <pthread.h>
#ifndef RC_INPUT_MAX_CHANNELS
#define RC_INPUT_MAX_CHANNELS 18
#endif
class VRBRAIN::VRBRAINRCInput : public AP_HAL::RCInput {
public:
void init();
@ -19,6 +24,8 @@ public:
void _timer_tick(void);
bool rc_bind(int dsmMode);
private:
/* override state */
uint16_t _override[RC_INPUT_MAX_CHANNELS];

449
libraries/AP_HAL_VRBRAIN/RCOutput.cpp
View File

@ -11,17 +11,27 @@
#include <unistd.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/topics/actuator_direct.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_sbus.h>
extern const AP_HAL::HAL& hal;
using namespace VRBRAIN;
/*
enable RCOUT_DEBUG_LATENCY to measure output latency using a logic
analyser. AUX6 will go high during the cork/push output.
*/
#define RCOUT_DEBUG_LATENCY 0
void VRBRAINRCOutput::init()
{
_perf_rcout = perf_alloc(PC_ELAPSED, "APM_rcout");
_pwm_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
_pwm_fd = open(PWM_OUTPUT0_DEVICE_PATH, O_RDWR);
if (_pwm_fd == -1) {
AP_HAL::panic("Unable to open " PWM_OUTPUT_DEVICE_PATH);
AP_HAL::panic("Unable to open " PWM_OUTPUT0_DEVICE_PATH);
}
if (ioctl(_pwm_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming\n");
@ -29,47 +39,98 @@ void VRBRAINRCOutput::init()
if (ioctl(_pwm_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup IO arming OK\n");
}
_rate_mask = 0;
_alt_fd = -1;
_servo_count = 0;
_alt_servo_count = 0;
if (ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
_outputs[i].pwm_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), i);
}
// _alt_fd = open("/dev/px4fmu", O_RDWR);
// if (_alt_fd == -1) {
// hal.console->printf("RCOutput: failed to open /dev/px4fmu");
// return;
// }
// ensure not to write zeros to disabled channels
_enabled_channels = 0;
for (uint8_t i=0; i < VRBRAIN_NUM_OUTPUT_CHANNELS; i++) {
_period[i] = PWM_IGNORE_THIS_CHANNEL;
}
// publish actuator vaules on demand
_actuator_direct_pub = NULL;
// and armed state
_actuator_armed_pub = NULL;
}
void VRBRAINRCOutput::_init_alt_channels(void)
{
if (_alt_fd == -1) {
return;
}
if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
return;
}
if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
return;
}
if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
}
}
void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
/*
set output frequency on outputs associated with fd
*/
void VRBRAINRCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz)
{
// we can't set this per channel yet
if (freq_hz > 50) {
// we can't set this per channel
if (freq_hz > 50 || freq_hz == 1) {
// we're being asked to set the alt rate
if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
if (_output_mode == MODE_PWM_ONESHOT) {
/*
set a 1Hz update for oneshot. This periodic output will
never actually trigger, instead we will directly trigger
the pulse after each push()
*/
freq_hz = 1;
}
//::printf("SET_UPDATE_RATE %u\n", (unsigned)freq_hz);
if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
return;
}
_freq_hz = freq_hz;
}
/* work out the new rate mask. The PX4IO board has 3 groups of servos.
/* work out the new rate mask. The outputs have 4 groups of servos.
Group 0: channels 0 1
Group 1: channels 4 5 6 7
Group 2: channels 2 3
Group 0: channels 0 1 2
Group 1: channels 3 4 5
Group 2: channels 6 7
Group 8: channels 8 9 10 11
Channels within a group must be set to the same rate.
For the moment we never set the channels above 8 to more than
50Hz
*/
if (freq_hz > 50) {
if (freq_hz > 50 || freq_hz == 1) {
// we are setting high rates on the given channels
_rate_mask |= chmask & 0xFF;
if (_rate_mask & 0x07) {
@ -81,6 +142,9 @@ void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
if (_rate_mask & 0xC0) {
_rate_mask |= 0xC0;
}
if (_rate_mask & 0xF00) {
_rate_mask |= 0xF00;
}
} else {
// we are setting low rates on the given channels
if (chmask & 0x07) {
@ -92,13 +156,52 @@ void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
if (chmask & 0xC0) {
_rate_mask &= ~0xC0;
}
if (chmask & 0xF00) {
_rate_mask &= ~0xF00;
}
}
if (ioctl(_pwm_fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
//::printf("SELECT_UPDATE_RATE 0x%02x\n", (unsigned)_rate_mask);
if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
}
}
/*
set output frequency
*/
void VRBRAINRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
{
if (freq_hz > 50 && _output_mode == MODE_PWM_ONESHOT) {
// rate is irrelevent in oneshot
return;
}
// re-fetch servo count as it might have changed due to a change in BRD_PWM_COUNT
if (_pwm_fd != -1 && ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get servo count\n");
return;
}
if (_alt_fd != -1 && ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
hal.console->printf("RCOutput: Unable to get alt servo count\n");
return;
}
// greater than 400 doesn't give enough room at higher periods for
// the down pulse
if (freq_hz > 400) {
freq_hz = 400;
}
uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
uint32_t alt_mask = chmask >> _servo_count;
if (primary_mask && _pwm_fd != -1) {
set_freq_fd(_pwm_fd, primary_mask, freq_hz);
}
if (alt_mask && _alt_fd != -1) {
set_freq_fd(_alt_fd, alt_mask, freq_hz);
}
}
uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
{
if (_rate_mask & (1U<<ch)) {
@ -109,12 +212,29 @@ uint16_t VRBRAINRCOutput::get_freq(uint8_t ch)
void VRBRAINRCOutput::enable_ch(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
// this is the first enable of an auxiliary channel - setup
// aux channels now. This delayed setup makes it possible to
// use BRD_PWM_COUNT to setup the number of PWM channels.
_init_alt_channels();
}
_enabled_channels |= (1U<<ch);
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
_period[ch] = 0;
}
}
void VRBRAINRCOutput::disable_ch(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
_enabled_channels &= ~(1U<<ch);
_period[ch] = PWM_IGNORE_THIS_CHANNEL;
}
void VRBRAINRCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
@ -151,21 +271,48 @@ void VRBRAINRCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us)
bool VRBRAINRCOutput::force_safety_on(void)
{
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
return (ret == OK);
_safety_state_request = AP_HAL::Util::SAFETY_DISARMED;
_safety_state_request_last_ms = 1;
return true;
}
void VRBRAINRCOutput::force_safety_off(void)
{
int ret = ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if (ret != OK) {
hal.console->printf("Failed to force safety off\n");
_safety_state_request = AP_HAL::Util::SAFETY_ARMED;
_safety_state_request_last_ms = 1;
}
void VRBRAINRCOutput::force_safety_pending_requests(void)
{
// check if there is a pending saftey_state change. If so (timer != 0) then set it.
if (_safety_state_request_last_ms != 0 &&
AP_HAL::millis() - _safety_state_request_last_ms >= 100) {
if (hal.util->safety_switch_state() == _safety_state_request) {
// current switch state matches request, stop attempting
_safety_state_request_last_ms = 0;
} else if (_safety_state_request == AP_HAL::Util::SAFETY_DISARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
_safety_state_request_last_ms = AP_HAL::millis();
} else if (_safety_state_request == AP_HAL::Util::SAFETY_ARMED) {
// current != requested, set it
ioctl(_pwm_fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
_safety_state_request_last_ms = AP_HAL::millis();
}
}
}
void VRBRAINRCOutput::force_safety_no_wait(void)
{
if (_safety_state_request_last_ms != 0) {
_safety_state_request_last_ms = 1;
force_safety_pending_requests();
}
}
void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
{
if (ch >= _servo_count) {
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return;
}
if (!(_enabled_channels & (1U<<ch))) {
@ -175,7 +322,13 @@ void VRBRAINRCOutput::write(uint8_t ch, uint16_t period_us)
if (ch >= _max_channel) {
_max_channel = ch + 1;
}
if (period_us != _period[ch]) {
/*
only mark an update is needed if there has been a change, or we
are in oneshot mode. In oneshot mode we always need to send the
output
*/
if (period_us != _period[ch] ||
_output_mode == MODE_PWM_ONESHOT) {
_period[ch] = period_us;
_need_update = true;
up_pwm_servo_set(ch, period_us);
@ -187,6 +340,16 @@ uint16_t VRBRAINRCOutput::read(uint8_t ch)
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return 0;
}
// if px4io has given us a value for this channel use that,
// otherwise use the value we last sent. This makes it easier to
// observe the behaviour of failsafe in px4io
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
if (_outputs[i].pwm_sub >= 0 &&
ch < _outputs[i].outputs.noutputs &&
_outputs[i].outputs.output[ch] > 0) {
return _outputs[i].outputs.output[ch];
}
}
return _period[ch];
}
@ -197,23 +360,257 @@ void VRBRAINRCOutput::read(uint16_t* period_us, uint8_t len)
}
}
void VRBRAINRCOutput::_timer_tick(void)
uint16_t VRBRAINRCOutput::read_last_sent(uint8_t ch)
{
if (ch >= VRBRAIN_NUM_OUTPUT_CHANNELS) {
return 0;
}
return _period[ch];
}
void VRBRAINRCOutput::read_last_sent(uint16_t* period_us, uint8_t len)
{
for (uint8_t i=0; i<len; i++) {
period_us[i] = read_last_sent(i);
}
}
/*
update actuator armed state
*/
void VRBRAINRCOutput::_arm_actuators(bool arm)
{
if (_armed.armed == arm) {
// already armed;
return;
}
_armed.timestamp = hrt_absolute_time();
_armed.armed = arm;
_armed.ready_to_arm = arm;
_armed.lockdown = false;
_armed.force_failsafe = false;
if (_actuator_armed_pub == NULL) {
_actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
} else {
orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
}
}
/*
publish new outputs to the actuator_direct topic
*/
void VRBRAINRCOutput::_publish_actuators(void)
{
struct actuator_direct_s actuators;
if (_esc_pwm_min == 0 ||
_esc_pwm_max == 0) {
// not initialised yet
return;
}
actuators.nvalues = _max_channel;
if (actuators.nvalues > actuators.NUM_ACTUATORS_DIRECT) {
actuators.nvalues = actuators.NUM_ACTUATORS_DIRECT;
}
// don't publish more than 8 actuators for now, as the uavcan ESC
// driver refuses to update any motors if you try to publish more
// than 8
if (actuators.nvalues > 8) {
actuators.nvalues = 8;
}
bool armed = hal.util->get_soft_armed();
actuators.timestamp = hrt_absolute_time();
for (uint8_t i=0; i<actuators.nvalues; i++) {
if (!armed) {
actuators.values[i] = 0;
} else {
actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
}
// actuator values are from -1 to 1
actuators.values[i] = actuators.values[i]*2 - 1;
}
if (_actuator_direct_pub == NULL) {
_actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
} else {
orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
_arm_actuators(true);
}
}
void VRBRAINRCOutput::_send_outputs(void)
{
uint32_t now = AP_HAL::micros();
if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) {
// no channels enabled
_arm_actuators(false);
goto update_pwm;
}
// always send at least at 20Hz, otherwise the IO board may think
// we are dead
if (now - _last_output > 50000) {
_need_update = true;
}
// check for PWM count changing. This can happen then the user changes BRD_PWM_COUNT
if (now - _last_config_us > 1000000) {
if (_pwm_fd != -1) {
ioctl(_pwm_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_servo_count);
}
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count);
}
_last_config_us = now;
}
if (_need_update && _pwm_fd != -1) {
_need_update = false;
perf_begin(_perf_rcout);
::write(_pwm_fd, _period, _max_channel*sizeof(_period[0]));
perf_end(_perf_rcout);
_last_output = now;
uint8_t to_send = _max_channel<_servo_count?_max_channel:_servo_count;
if (_sbus_enabled) {
to_send = _max_channel;
}
if (to_send > 0) {
for (int i=to_send-1; i >= 0; i--) {
if (_period[i] == 0 || _period[i] == PWM_IGNORE_THIS_CHANNEL) {
to_send = i;
} else {
break;
}
}
}
if (to_send > 0) {
::write(_pwm_fd, _period, to_send*sizeof(_period[0]));
}
if (_max_channel > _servo_count) {
// maybe send updates to alt_fd
if (_alt_fd != -1 && _alt_servo_count > 0) {
uint8_t n = _max_channel - _servo_count;
if (n > _alt_servo_count) {
n = _alt_servo_count;
}
if (n > 0) {
::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
}
}
}
// also publish to actuator_direct
_publish_actuators();
perf_end(_perf_rcout);
_last_output = now;
}
update_pwm:
for (uint8_t i=0; i<ORB_MULTI_MAX_INSTANCES; i++) {
bool rc_updated = false;
if (_outputs[i].pwm_sub >= 0 &&
orb_check(_outputs[i].pwm_sub, &rc_updated) == 0 &&
rc_updated) {
orb_copy(ORB_ID(actuator_outputs), _outputs[i].pwm_sub, &_outputs[i].outputs);
}
}
}
void VRBRAINRCOutput::cork()
{
#if RCOUT_DEBUG_LATENCY
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 1);
#endif
_corking = true;
}
void VRBRAINRCOutput::push()
{
#if RCOUT_DEBUG_LATENCY
hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
hal.gpio->write(55, 0);
#endif
_corking = false;
if (_output_mode == MODE_PWM_ONESHOT) {
// run timer immediately in oneshot mode
_send_outputs();
}
}
void VRBRAINRCOutput::_timer_tick(void)
{
if (_output_mode != MODE_PWM_ONESHOT) {
/* in oneshot mode the timer does nothing as the outputs are
* sent from push() */
_send_outputs();
}
force_safety_pending_requests();
}
/*
enable sbus output
*/
bool VRBRAINRCOutput::enable_sbus_out(uint16_t rate_hz)
{
int fd = open("/dev/px4io", 0);
if (fd == -1) {
return false;
}
for (uint8_t tries=0; tries<10; tries++) {
if (ioctl(fd, SBUS_SET_PROTO_VERSION, 1) != 0) {
continue;
}
if (ioctl(fd, PWM_SERVO_SET_SBUS_RATE, rate_hz) != 0) {
continue;
}
close(fd);
_sbus_enabled = true;
return true;
}
close(fd);
return false;
}
/*
setup output mode
*/
void VRBRAINRCOutput::set_output_mode(enum output_mode mode)
{
if (_output_mode == mode) {
// no change
return;
}
if (mode == MODE_PWM_ONESHOT) {
// when using oneshot we don't want the regular pulses. The
// best we can do with the current PX4Firmware code is ask for
// 1Hz. This does still produce pulses, but the trigger calls
// mean the timer is constantly reset, so no pulses are
// produced except when triggered by push() when the main loop
// is running
set_freq(_rate_mask, 1);
}
_output_mode = mode;
if (_output_mode == MODE_PWM_ONESHOT) {
//::printf("enable oneshot\n");
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 1);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 1);
}
} else {
ioctl(_pwm_fd, PWM_SERVO_SET_ONESHOT, 0);
if (_alt_fd != -1) {
ioctl(_alt_fd, PWM_SERVO_SET_ONESHOT, 0);
}
}
}
#endif // CONFIG_HAL_BOARD

63
libraries/AP_HAL_VRBRAIN/RCOutput.h
View File

@ -2,40 +2,75 @@
#include "AP_HAL_VRBRAIN.h"
#include <systemlib/perf_counter.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#define VRBRAIN_NUM_OUTPUT_CHANNELS 16
class VRBRAIN::VRBRAINRCOutput : public AP_HAL::RCOutput
{
public:
void init();
void set_freq(uint32_t chmask, uint16_t freq_hz);
uint16_t get_freq(uint8_t ch);
void enable_ch(uint8_t ch);
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us);
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void set_safety_pwm(uint32_t chmask, uint16_t period_us);
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us);
bool force_safety_on(void);
void force_safety_off(void);
void init() override;
void set_freq(uint32_t chmask, uint16_t freq_hz) override;
uint16_t get_freq(uint8_t ch) override;
void enable_ch(uint8_t ch) override;
void disable_ch(uint8_t ch) override;
void write(uint8_t ch, uint16_t period_us) override;
uint16_t read(uint8_t ch) override;
void read(uint16_t* period_us, uint8_t len) override;
uint16_t read_last_sent(uint8_t ch) override;
void read_last_sent(uint16_t* period_us, uint8_t len) override;
void set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
bool force_safety_on(void) override;
void force_safety_off(void) override;
void force_safety_no_wait(void) override;
void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
_esc_pwm_min = min_pwm;
_esc_pwm_max = max_pwm;
}
void cork();
void push();
void set_output_mode(enum output_mode mode) override;
void _timer_tick(void);
bool enable_sbus_out(uint16_t rate_hz) override;
private:
int _pwm_fd;
int _alt_fd;
uint16_t _freq_hz;
uint16_t _period[VRBRAIN_NUM_OUTPUT_CHANNELS];
volatile uint8_t _max_channel;
volatile bool _need_update;
bool _sbus_enabled:1;
perf_counter_t _perf_rcout;
uint32_t _last_output;
uint32_t _last_config_us;
unsigned _servo_count;
unsigned _alt_servo_count;
uint32_t _rate_mask;
uint16_t _enabled_channels;
struct {
int pwm_sub;
actuator_outputs_s outputs;
} _outputs[ORB_MULTI_MAX_INSTANCES] {};
actuator_armed_s _armed;
orb_advert_t _actuator_direct_pub = NULL;
orb_advert_t _actuator_armed_pub = NULL;
uint16_t _esc_pwm_min = 0;
uint16_t _esc_pwm_max = 0;
void _init_alt_channels(void);
void _publish_actuators(void);
void _arm_actuators(bool arm);
void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
bool _corking;
enum output_mode _output_mode = MODE_PWM_NORMAL;
void _send_outputs(void);
enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
uint32_t _safety_state_request_last_ms = 0;
void force_safety_pending_requests(void);
};

129
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
View File

@ -33,6 +33,7 @@ extern bool _vrbrain_thread_should_exit;
VRBRAINScheduler::VRBRAINScheduler() :
_perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")),
_perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")),
_perf_storage_timer(perf_alloc(PC_ELAPSED, "APM_storage_timers")),
_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
{}
@ -51,7 +52,7 @@ void VRBRAINScheduler::init()
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_timer_thread, this);
pthread_create(&_timer_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_timer_thread, this);
// the UART thread runs at a medium priority
pthread_attr_init(&thread_attr);
@ -61,7 +62,7 @@ void VRBRAINScheduler::init()
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_uart_thread, this);
pthread_create(&_uart_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_uart_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
@ -71,7 +72,17 @@ void VRBRAINScheduler::init()
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&VRBRAIN::VRBRAINScheduler::_io_thread, this);
pthread_create(&_io_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_io_thread, this);
// the storage thread runs at just above IO priority
pthread_attr_init(&thread_attr);
pthread_attr_setstacksize(&thread_attr, 1024);
param.sched_priority = APM_STORAGE_PRIORITY;
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_storage_thread_ctx, &thread_attr, &VRBRAIN::VRBRAINScheduler::_storage_thread, this);
}
/**
@ -82,6 +93,7 @@ void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec)
sem_t wait_semaphore;
struct hrt_call wait_call;
sem_init(&wait_semaphore, 0, 0);
memset(&wait_call, 0, sizeof(wait_call));
hrt_call_after(&wait_call, usec, (hrt_callout)sem_post, &wait_semaphore);
sem_wait(&wait_semaphore);
}
@ -89,17 +101,41 @@ void VRBRAINScheduler::delay_microseconds_semaphore(uint16_t usec)
void VRBRAINScheduler::delay_microseconds(uint16_t usec)
{
perf_begin(_perf_delay);
if (usec >= 500) {
delay_microseconds_semaphore(usec);
perf_end(_perf_delay);
return;
}
uint64_t start = AP_HAL::micros64();
uint64_t dt;
while ((dt=(AP_HAL::micros64() - start)) < usec) {
up_udelay(usec - dt);
/*
wrapper around sem_post that boosts main thread priority
*/
static void sem_post_boost(sem_t *sem)
{
hal_vrbrain_set_priority(APM_MAIN_PRIORITY_BOOST);
sem_post(sem);
}
perf_end(_perf_delay);
/*
return the main thread to normal priority
*/
static void set_normal_priority(void *sem)
{
hal_vrbrain_set_priority(APM_MAIN_PRIORITY);
}
/*
a variant of delay_microseconds that boosts priority to
APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC
microseconds when the time completes. This significantly improves
the regularity of timing of the main loop as it takes
*/
void VRBRAINScheduler::delay_microseconds_boost(uint16_t usec)
{
sem_t wait_semaphore;
static struct hrt_call wait_call;
sem_init(&wait_semaphore, 0, 0);
hrt_call_after(&wait_call, usec, (hrt_callout)sem_post_boost, &wait_semaphore);
sem_wait(&wait_semaphore);
hrt_call_after(&wait_call, APM_MAIN_PRIORITY_BOOST_USEC, (hrt_callout)set_normal_priority, NULL);
}
void VRBRAINScheduler::delay(uint16_t ms)
@ -186,7 +222,14 @@ void VRBRAINScheduler::resume_timer_procs()
void VRBRAINScheduler::reboot(bool hold_in_bootloader)
{
systemreset(hold_in_bootloader);
// disarm motors to ensure they are off during a bootloader upload
hal.rcout->force_safety_on();
hal.rcout->force_safety_no_wait();
// delay to ensure the async force_saftey operation completes
delay(500);
px4_systemreset(hold_in_bootloader);
}
void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
@ -208,7 +251,7 @@ void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
}
// and the failsafe, if one is setup
if (_failsafe) {
if (_failsafe != NULL) {
_failsafe();
}
@ -220,19 +263,21 @@ void VRBRAINScheduler::_run_timers(bool called_from_timer_thread)
extern bool vrbrain_ran_overtime;
void *VRBRAINScheduler::_timer_thread(void)
void *VRBRAINScheduler::_timer_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
uint32_t last_ran_overtime = 0;
while (!_hal_initialized) {
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
delay_microseconds_semaphore(1000);
sched->delay_microseconds_semaphore(1000);
// run registered timers
perf_begin(_perf_timers);
_run_timers(true);
perf_end(_perf_timers);
perf_begin(sched->_perf_timers);
sched->_run_timers(true);
perf_end(sched->_perf_timers);
// process any pending RC output requests
((VRBRAINRCOutput *)hal.rcout)->_timer_tick();
@ -242,8 +287,10 @@ void *VRBRAINScheduler::_timer_thread(void)
if (vrbrain_ran_overtime && AP_HAL::millis() - last_ran_overtime > 2000) {
last_ran_overtime = AP_HAL::millis();
// printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
// hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
#if 0
printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
hal.console->printf("Overtime in task %d\n", (int)AP_Scheduler::current_task);
#endif
}
}
return NULL;
@ -268,13 +315,15 @@ void VRBRAINScheduler::_run_io(void)
_in_io_proc = false;
}
void *VRBRAINScheduler::_uart_thread(void)
void *VRBRAINScheduler::_uart_thread(void *arg)
{
while (!_hal_initialized) {
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
delay_microseconds_semaphore(1000);
sched->delay_microseconds_semaphore(1000);
// process any pending serial bytes
((VRBRAINUARTDriver *)hal.uartA)->_timer_tick();
@ -282,25 +331,43 @@ void *VRBRAINScheduler::_uart_thread(void)
((VRBRAINUARTDriver *)hal.uartC)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartD)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartE)->_timer_tick();
((VRBRAINUARTDriver *)hal.uartF)->_timer_tick();
}
return NULL;
}
void *VRBRAINScheduler::_io_thread(void)
void *VRBRAINScheduler::_io_thread(void *arg)
{
while (!_hal_initialized) {
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
poll(NULL, 0, 1);
// process any pending storage writes
((VRBRAINStorage *)hal.storage)->_timer_tick();
// run registered IO processes
perf_begin(_perf_io_timers);
_run_io();
perf_end(_perf_io_timers);
perf_begin(sched->_perf_io_timers);
sched->_run_io();
perf_end(sched->_perf_io_timers);
}
return NULL;
}
void *VRBRAINScheduler::_storage_thread(void *arg)
{
VRBRAINScheduler *sched = (VRBRAINScheduler *)arg;
while (!sched->_hal_initialized) {
poll(NULL, 0, 1);
}
while (!_vrbrain_thread_should_exit) {
poll(NULL, 0, 10);
// process any pending storage writes
perf_begin(sched->_perf_storage_timer);
((VRBRAINStorage *)hal.storage)->_timer_tick();
perf_end(sched->_perf_storage_timer);
}
return NULL;
}

32
libraries/AP_HAL_VRBRAIN/Scheduler.h
View File

@ -10,13 +10,33 @@
#define VRBRAIN_SCHEDULER_MAX_TIMER_PROCS 8
#define APM_MAIN_PRIORITY_BOOST 241
#define APM_MAIN_PRIORITY 180
#define APM_TIMER_PRIORITY 181
#define APM_UART_PRIORITY 60
#define APM_IO_PRIORITY 59
#define APM_STORAGE_PRIORITY 59
#define APM_IO_PRIORITY 58
#define APM_SHELL_PRIORITY 57
#define APM_OVERTIME_PRIORITY 10
#define APM_STARTUP_PRIORITY 10
/* how long to boost priority of the main thread for each main
loop. This needs to be long enough for all interrupt-level drivers
(mostly SPI drivers) to run, and for the main loop of the vehicle
code to start the AHRS update.
Priority boosting of the main thread in delay_microseconds_boost()
avoids the problem that drivers in hpwork all happen to run right
at the start of the period where the main vehicle loop is calling
wait_for_sample(). That causes main loop timing jitter, which
reduces performance. Using the priority boost the main loop
temporarily runs at a priority higher than hpwork and the timer
thread, which results in much more consistent loop timing.
*/
#define APM_MAIN_PRIORITY_BOOST_USEC 150
#define APM_MAIN_THREAD_STACK_SIZE 8192
/* Scheduler implementation: */
class VRBRAIN::VRBRAINScheduler : public AP_HAL::Scheduler {
public:
@ -26,6 +46,7 @@ public:
void init();
void delay(uint16_t ms);
void delay_microseconds(uint16_t us);
void delay_microseconds_boost(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::MemberProc);
@ -60,11 +81,13 @@ private:
pid_t _main_task_pid;
pthread_t _timer_thread_ctx;
pthread_t _io_thread_ctx;
pthread_t _storage_thread_ctx;
pthread_t _uart_thread_ctx;
void *_timer_thread(void);
void *_io_thread(void);
void *_uart_thread(void);
static void *_timer_thread(void *arg);
static void *_io_thread(void *arg);
static void *_storage_thread(void *arg);
static void *_uart_thread(void *arg);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);
@ -73,6 +96,7 @@ private:
perf_counter_t _perf_timers;
perf_counter_t _perf_io_timers;
perf_counter_t _perf_storage_timer;
perf_counter_t _perf_delay;
};
#endif

39
libraries/AP_HAL_VRBRAIN/Semaphores.cpp Normal file
View File

@ -0,0 +1,39 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "Semaphores.h"
extern const AP_HAL::HAL& hal;
using namespace VRBRAIN;
bool Semaphore::give()
{
return pthread_mutex_unlock(&_lock) == 0;
}
bool Semaphore::take(uint32_t timeout_ms)
{
if (timeout_ms == 0) {
return pthread_mutex_lock(&_lock) == 0;
}
if (take_nonblocking()) {
return true;
}
uint64_t start = AP_HAL::micros64();
do {
hal.scheduler->delay_microseconds(200);
if (take_nonblocking()) {
return true;
}
} while ((AP_HAL::micros64() - start) < timeout_ms*1000);
return false;
}
bool Semaphore::take_nonblocking()
{
return pthread_mutex_trylock(&_lock) == 0;
}
#endif // CONFIG_HAL_BOARD

20
libraries/AP_HAL_VRBRAIN/Semaphores.h Normal file
View File

@ -0,0 +1,20 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "AP_HAL_VRBRAIN.h"
#include <pthread.h>
class VRBRAIN::Semaphore : public AP_HAL::Semaphore {
public:
Semaphore() {
pthread_mutex_init(&_lock, NULL);
}
bool give();
bool take(uint32_t timeout_ms);
bool take_nonblocking();
private:
pthread_mutex_t _lock;
};
#endif // CONFIG_HAL_BOARD

43
libraries/AP_HAL_VRBRAIN/Storage.cpp
View File

@ -23,6 +23,7 @@ using namespace VRBRAIN;
#define STORAGE_DIR "/fs/microsd/APM"
#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak"
//#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
#define MTD_PARAMS_FILE "/fs/mtd"
#define MTD_SIGNATURE 0x14012014
#define MTD_SIGNATURE_OFFSET (8192-4)
@ -51,9 +52,11 @@ uint32_t VRBRAINStorage::_mtd_signature(void)
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
}
bus_lock(true);
if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
AP_HAL::panic("Failed to read signature from " MTD_PARAMS_FILE);
}
bus_lock(false);
close(mtd_fd);
return v;
}
@ -71,9 +74,11 @@ void VRBRAINStorage::_mtd_write_signature(void)
if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
AP_HAL::panic("Failed to seek in " MTD_PARAMS_FILE);
}
bus_lock(true);
if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
AP_HAL::panic("Failed to write signature in " MTD_PARAMS_FILE);
}
bus_lock(false);
close(mtd_fd);
}
@ -103,10 +108,12 @@ void VRBRAINStorage::_upgrade_to_mtd(void)
}
close(old_fd);
ssize_t ret;
bus_lock(true);
if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
AP_HAL::panic("Unable to write MTD for upgrade");
}
bus_lock(false);
close(mtd_fd);
#if STORAGE_RENAME_OLD_FILE
rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
@ -156,7 +163,9 @@ void VRBRAINStorage::_storage_open(void)
}
const uint16_t chunk_size = 128;
for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
bus_lock(true);
ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
bus_lock(false);
if (ret != chunk_size) {
::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
(unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
@ -164,6 +173,15 @@ void VRBRAINStorage::_storage_open(void)
}
}
close(fd);
#ifdef SAVE_STORAGE_FILE
fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (fd != -1) {
write(fd, _buffer, sizeof(_buffer));
close(fd);
::printf("Saved storage file %s\n", SAVE_STORAGE_FILE);
}
#endif
_initialised = true;
}
@ -203,6 +221,26 @@ void VRBRAINStorage::write_block(uint16_t loc, const void *src, size_t n)
memcpy(&_buffer[loc], src, n);
_mark_dirty(loc, n);
}
}
void VRBRAINStorage::bus_lock(bool lock)
{
}
void VRBRAINStorage::_timer_tick(void)
@ -254,7 +292,10 @@ void VRBRAINStorage::_timer_tick(void)
*/
if (lseek(_fd, i<<VRBRAIN_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<VRBRAIN_STORAGE_LINE_SHIFT)) {
_dirty_mask &= ~write_mask;
if (write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT) != n<<VRBRAIN_STORAGE_LINE_SHIFT) {
bus_lock(true);
ssize_t ret = write(_fd, &_buffer[i<<VRBRAIN_STORAGE_LINE_SHIFT], n<<VRBRAIN_STORAGE_LINE_SHIFT);
bus_lock(false);
if (ret != n<<VRBRAIN_STORAGE_LINE_SHIFT) {
// write error - likely EINTR
_dirty_mask |= write_mask;
close(_fd);

5
libraries/AP_HAL_VRBRAIN/Storage.h
View File

@ -34,4 +34,9 @@ private:
void _upgrade_to_mtd(void);
uint32_t _mtd_signature(void);
void _mtd_write_signature(void);
void bus_lock(bool lock);
};

31
libraries/AP_HAL_VRBRAIN/UARTDriver.cpp
View File

@ -17,6 +17,7 @@
#include <drivers/drv_hrt.h>
#include <assert.h>
#include <AP_HAL/utility/RingBuffer.h>
#include "GPIO.h"
using namespace VRBRAIN;
@ -51,7 +52,7 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
uint16_t min_tx_buffer = 1024;
uint16_t min_rx_buffer = 512;
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
min_tx_buffer = 16384;
min_tx_buffer = 4096;
min_rx_buffer = 1024;
}
// on VRBRAIN we have enough memory to have a larger transmit and
@ -133,14 +134,19 @@ void VRBRAINUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) {
if (!_initialised) {
if (strcmp(_devpath, "/dev/ttyACM0") == 0) {
((VRBRAINGPIO *)hal.gpio)->set_usb_connected();
}
::printf("initialised %s OK %u %u\n", _devpath,
(unsigned)_writebuf_size, (unsigned)_readbuf_size);
}
_initialised = true;
}
_uart_owner_pid = getpid();
}
void VRBRAINUARTDriver::set_flow_control(enum flow_control flow_control)
void VRBRAINUARTDriver::set_flow_control(enum flow_control fcontrol)
{
if (_fd == -1) {
return;
@ -148,7 +154,7 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control flow_control)
struct termios t;
tcgetattr(_fd, &t);
// we already enabled CRTS_IFLOW above, just enable output flow control
if (flow_control != FLOW_CONTROL_DISABLE) {
if (fcontrol != FLOW_CONTROL_DISABLE) {
t.c_cflag |= CRTSCTS;
} else {
t.c_cflag &= ~CRTSCTS;
@ -159,7 +165,7 @@ void VRBRAINUARTDriver::set_flow_control(enum flow_control flow_control)
_total_written = 0;
_first_write_time = 0;
}
_flow_control = flow_control;
_flow_control = fcontrol;
}
void VRBRAINUARTDriver::begin(uint32_t b)
@ -259,6 +265,9 @@ uint32_t VRBRAINUARTDriver::txspace()
int16_t VRBRAINUARTDriver::read()
{
uint8_t c;
if (_uart_owner_pid != getpid()){
return -1;
}
if (!_initialised) {
try_initialise();
return -1;
@ -279,12 +288,11 @@ int16_t VRBRAINUARTDriver::read()
*/
size_t VRBRAINUARTDriver::write(uint8_t c)
{
if (!_initialised) {
try_initialise();
if (_uart_owner_pid != getpid()){
return 0;
}
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
if (!_initialised) {
try_initialise();
return 0;
}
uint16_t _head;
@ -305,12 +313,11 @@ size_t VRBRAINUARTDriver::write(uint8_t c)
*/
size_t VRBRAINUARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!_initialised) {
try_initialise();
if (_uart_owner_pid != getpid()){
return 0;
}
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
if (!_initialised) {
try_initialise();
return 0;
}

3
libraries/AP_HAL_VRBRAIN/UARTDriver.h
View File

@ -74,4 +74,7 @@ private:
uint32_t _total_read;
uint32_t _total_written;
enum flow_control _flow_control;
pid_t _uart_owner_pid;
};

93
libraries/AP_HAL_VRBRAIN/Util.cpp
View File

@ -12,6 +12,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <systemlib/board_serial.h>
#include <drivers/drv_gpio.h>
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL& hal;
@ -116,8 +118,10 @@ bool VRBRAINUtil::get_system_id(char buf[40])
const char *board_type = "VRUBRAINv51";
#elif defined(CONFIG_ARCH_BOARD_VRUBRAIN_V52)
const char *board_type = "VRUBRAINv52";
#elif defined(CONFIG_ARCH_BOARD_VRHERO_V10)
const char *board_type = "VRHEROv10";
#elif defined(CONFIG_ARCH_BOARD_VRCORE_V10)
const char *board_type = "VRCOREv10";
#elif defined(CONFIG_ARCH_BOARD_VRBRAIN_V54)
const char *board_type = "VRBRAINv54";
#endif
// this format is chosen to match the human_readable_serial()
// function in auth.c
@ -137,4 +141,89 @@ uint32_t VRBRAINUtil::available_memory(void)
return mallinfo().fordblks;
}
/*
AP_HAL wrapper around PX4 perf counters
*/
VRBRAINUtil::perf_counter_t VRBRAINUtil::perf_alloc(VRBRAINUtil::perf_counter_type t, const char *name)
{
::perf_counter_type vrbrain_t;
switch (t) {
case VRBRAINUtil::PC_COUNT:
vrbrain_t = ::PC_COUNT;
break;
case VRBRAINUtil::PC_ELAPSED:
vrbrain_t = ::PC_ELAPSED;
break;
case VRBRAINUtil::PC_INTERVAL:
vrbrain_t = ::PC_INTERVAL;
break;
default:
return NULL;
}
return (perf_counter_t)::perf_alloc(vrbrain_t, name);
}
void VRBRAINUtil::perf_begin(perf_counter_t h)
{
::perf_begin((::perf_counter_t)h);
}
void VRBRAINUtil::perf_end(perf_counter_t h)
{
::perf_end((::perf_counter_t)h);
}
void VRBRAINUtil::perf_count(perf_counter_t h)
{
::perf_count((::perf_counter_t)h);
}
void VRBRAINUtil::set_imu_temp(float current)
{
if (!_heater.target || *_heater.target == -1) {
return;
}
// average over temperatures to remove noise
_heater.count++;
_heater.sum += current;
// update once a second
uint32_t now = AP_HAL::millis();
if (now - _heater.last_update_ms < 1000) {
return;
}
_heater.last_update_ms = now;
current = _heater.sum / _heater.count;
_heater.sum = 0;
_heater.count = 0;
// experimentally tweaked for Pixhawk2
const float kI = 0.3f;
const float kP = 200.0f;
float err = ((float)*_heater.target) - current;
_heater.integrator += kI * err;
_heater.integrator = constrain_float(_heater.integrator, 0, 70);
float output = constrain_float(kP * err + _heater.integrator, 0, 100);
// hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", _heater.integrator, output, current, err);
if (_heater.fd == -1) {
_heater.fd = open("/dev/px4io", O_RDWR);
}
if (_heater.fd != -1) {
ioctl(_heater.fd, GPIO_SET_HEATER_DUTY_CYCLE, (unsigned)output);
}
}
void VRBRAINUtil::set_imu_target_temp(int8_t *target)
{
_heater.target = target;
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN

49
libraries/AP_HAL_VRBRAIN/Util.h
View File

@ -2,6 +2,30 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_VRBRAIN_Namespace.h"
#include "Semaphores.h"
class VRBRAIN::NSHShellStream : public AP_HAL::Stream {
public:
size_t write(uint8_t);
size_t write(const uint8_t *buffer, size_t size);
int16_t read() override;
uint32_t available() override;
uint32_t txspace() override;
private:
int shell_stdin = -1;
int shell_stdout = -1;
pthread_t shell_thread_ctx;
struct {
int in = -1;
int out = -1;
} child;
bool showed_memory_warning = false;
bool showed_armed_warning = false;
void start_shell(void);
static void shell_thread(void *arg);
};
class VRBRAIN::VRBRAINUtil : public AP_HAL::Util {
public:
@ -22,6 +46,31 @@ public:
uint32_t available_memory(void) override;
/*
return a stream for access to nsh shell
*/
AP_HAL::Stream *get_shell_stream() { return &_shell_stream; }
perf_counter_t perf_alloc(perf_counter_type t, const char *name) override;
void perf_begin(perf_counter_t ) override;
void perf_end(perf_counter_t) override;
void perf_count(perf_counter_t) override;
// create a new semaphore
AP_HAL::Semaphore *new_semaphore(void) override { return new VRBRAIN::Semaphore; }
void set_imu_temp(float current) override;
void set_imu_target_temp(int8_t *target) override;
private:
int _safety_handle;
VRBRAIN::NSHShellStream _shell_stream;
struct {
int8_t *target;
float integrator;
uint16_t count;
float sum;
uint32_t last_update_ms;
int fd = -1;
} _heater;
};

49
libraries/AP_HAL_VRBRAIN/vrbrain_param.cpp Normal file
View File

@ -0,0 +1,49 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This replaces the PX4Firmware parameter system with dummy
functions. The ArduPilot parameter system uses different formatting
for FRAM and we need to ensure that the PX4 parameter system doesn't
try to access FRAM in an invalid manner
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <px4_defines.h>
#include <px4_posix.h>
#include <stdio.h>
#include "systemlib/param/param.h"
#include "uORB/uORB.h"
#include "uORB/topics/parameter_update.h"
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
param_t param_find(const char *name)
{
::printf("VRBRAIN: param_find(%s)\n", name);
return PARAM_INVALID;
}
int param_get(param_t param, void *val)
{
return -1;
}
int param_set(param_t param, const void *val)
{
return -1;
}
int
param_set_no_notification(param_t param, const void *val)
{
return -1;
}
#endif // CONFIG_HAL_BOARD