dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #598 from PX4/rssi 

RSSI and concurrent S.Bus output handling
This commit is contained in:
Lorenz Meier 2014-01-23 09:13:53 -08:00
parent ac37172b52 6a40acdbdc
commit 046a71226b
9 changed files with 118 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -181,8 +181,8 @@ then
set IO_PRESENT yes
else
echo "[init] PX4IO CRC failure, trying to update"
echo "PX4IO CRC failure" >> $LOG_FILE
echo "[init] Trying to update"
echo "PX4IO Trying to update" >> $LOG_FILE
tone_alarm MLL32CP8MB

2
src/drivers/boards/px4io-v2/board_config.h
View File

@ -114,7 +114,7 @@
/* XXX these should be UART pins */
#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
/*
* High-resolution timer

2
src/drivers/boards/px4io-v2/px4iov2_init.c
View File

@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ADC_VSERVO);
stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
stm32_configgpio(GPIO_SBUS_OUTPUT);
/* sbus output enable is active low - disable it by default */

49
src/drivers/px4io/px4io.cpp
View File

@ -1454,8 +1454,10 @@ PX4IO::io_publish_raw_rc()
/* set RSSI */
// XXX the correct scaling needs to be validated here
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
// XXX the correct scaling needs to be validated here
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX;
}
/* lazily advertise on first publication */
if (_to_input_rc == 0) {
@ -1808,7 +1810,7 @@ PX4IO::print_status()
printf("\n");
if (raw_inputs > 0) {
if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
@ -2365,8 +2367,10 @@ start(int argc, char *argv[])
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr)
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
@ -2642,17 +2646,17 @@ monitor(void)
read(0, &c, 1);
if (cancels-- == 0) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
if (g_dev != nullptr) {
printf("\033[H"); /* move cursor home and clear screen */
printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
@ -2767,18 +2771,33 @@ px4io_main(int argc, char *argv[])
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
/* allocate the interface */
device::Device *interface = get_interface();
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
}
// tear down the px4io instance
delete g_dev;
if (OK != g_dev->init()) {
warnx("driver init failed, still trying..");
}
}
uint16_t arg = atol(argv[2]);
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
if (ret != OK) {
printf("reboot failed - %d\n", ret);
exit(1);
}
// tear down the px4io instance
delete g_dev;
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];

53
src/modules/px4iofirmware/adc.c
View File

@ -83,6 +83,14 @@ adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
/* bring the ADC out of power-down mode */
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
@ -96,41 +104,25 @@ adc_init(void)
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
/*
* Configure sampling time.
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
* datasheet).
*/
rSMPR1 = 0b00000000011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rSQR3 = 0; /* will be updated with the channel at conversion time */
return 0;
}
@ -141,11 +133,12 @@ adc_init(void)
uint16_t
adc_measure(unsigned channel)
{
perf_begin(adc_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
rSR = 0;
(void)rDR;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
@ -158,7 +151,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
perf_end(adc_perf);
return 0xffff;
}
@ -166,6 +158,7 @@ adc_measure(unsigned channel)
/* read the result and clear EOC */
uint16_t result = rDR;
rSR = 0;
perf_end(adc_perf);
return result;

11
src/modules/px4iofirmware/controls.c
View File

@ -114,9 +114,20 @@ controls_tick() {
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
}
/* switch S.Bus output pin as needed */
if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) {
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS));
#endif
}
perf_end(c_gather_sbus);
/*

55
src/modules/px4iofirmware/px4io.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
while (true) {
phase = !phase;
usleep(300000);
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
#endif
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
@ -270,7 +295,6 @@ user_start(int argc, char *argv[])
check_reboot();
#if 0
/* check for debug activity */
show_debug_messages();
@ -287,7 +311,6 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
#endif
}
}

2
src/modules/px4iofirmware/px4io.h
View File

@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit;
# define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
@ -183,6 +184,7 @@ extern void mixer_handle_text(const void *buffer, size_t length);
* Safety switch/LED.
*/
extern void safety_init(void);
extern void failsafe_led_init(void);
/**
* FMU communications

8
src/modules/px4iofirmware/safety.c
View File

@ -84,7 +84,11 @@ safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
}
void
failsafe_led_init(void)
{
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@ -165,8 +169,8 @@ failsafe_blink(void *arg)
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
return;
}
static bool failsafe = false;