Browse Source

Merge branch 'master' into beta

sbg
Anton Babushkin 11 years ago
parent
commit
b3d98e4a19
  1. 9
      ROMFS/px4fmu_common/init.d/rcS
  2. 2
      src/drivers/boards/px4io-v2/board_config.h
  3. 2
      src/drivers/boards/px4io-v2/px4iov2_init.c
  4. 4
      src/drivers/drv_pwm_output.h
  5. 160
      src/drivers/hmc5883/hmc5883.cpp
  6. 35
      src/drivers/px4fmu/fmu.cpp
  7. 49
      src/drivers/px4io/px4io.cpp
  8. 53
      src/modules/px4iofirmware/adc.c
  9. 11
      src/modules/px4iofirmware/controls.c
  10. 55
      src/modules/px4iofirmware/px4io.c
  11. 2
      src/modules/px4iofirmware/px4io.h
  12. 8
      src/modules/px4iofirmware/safety.c

9
ROMFS/px4fmu_common/init.d/rcS

@ -8,7 +8,6 @@
# #
set MODE autostart set MODE autostart
set LOG_FILE /fs/microsd/bootlog.txt
set RC_FILE /fs/microsd/etc/rc.txt set RC_FILE /fs/microsd/etc/rc.txt
set CONFIG_FILE /fs/microsd/etc/config.txt set CONFIG_FILE /fs/microsd/etc/config.txt
set EXTRAS_FILE /fs/microsd/etc/extras.txt set EXTRAS_FILE /fs/microsd/etc/extras.txt
@ -21,10 +20,12 @@ set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
echo "[init] Looking for microSD..." echo "[init] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd if mount -t vfat /dev/mmcsd0 /fs/microsd
then then
set LOG_FILE /fs/microsd/bootlog.txt
echo "[init] microSD card mounted at /fs/microsd" echo "[init] microSD card mounted at /fs/microsd"
# Start playing the startup tune # Start playing the startup tune
tone_alarm start tone_alarm start
else else
set LOG_FILE /dev/null
echo "[init] No microSD card found" echo "[init] No microSD card found"
# Play SOS # Play SOS
tone_alarm error tone_alarm error
@ -180,8 +181,8 @@ then
set IO_PRESENT yes set IO_PRESENT yes
else else
echo "[init] PX4IO CRC failure, trying to update" echo "[init] Trying to update"
echo "PX4IO CRC failure" >> $LOG_FILE echo "PX4IO Trying to update" >> $LOG_FILE
tone_alarm MLL32CP8MB tone_alarm MLL32CP8MB
@ -198,10 +199,12 @@ then
else else
echo "[init] ERROR: PX4IO update failed" echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi fi
else else
echo "[init] ERROR: PX4IO update failed" echo "[init] ERROR: PX4IO update failed"
echo "PX4IO update failed" >> $LOG_FILE echo "PX4IO update failed" >> $LOG_FILE
tone_alarm $TUNE_OUT_ERROR
fi fi
fi fi

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

@ -114,7 +114,7 @@
/* XXX these should be UART pins */ /* 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_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_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 * High-resolution timer

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

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

4
src/drivers/drv_pwm_output.h

@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm);
/** get the maximum PWM value the output will send */ /** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set the number of servos in (unsigned)arg - allows change of
* split between servos and GPIO */
#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20)
/** set a single servo to a specific value */ /** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

160
src/drivers/hmc5883/hmc5883.cpp

@ -849,42 +849,24 @@ HMC5883::collect()
/* scale values for output */ /* scale values for output */
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) { if (_bus == PX4_I2C_BUS_ONBOARD) {
/* to align the sensor axes with the board, x and y need to be flipped */ // convert onboard so it matches offboard for the
new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; // scaling below
/* flip axes and negate value for y */ report.y = -report.y;
new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; report.x = -report.x;
/* z remains z */ }
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
} else {
#endif
/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
* therefore switch x and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
#ifdef PX4_I2C_BUS_ONBOARD
}
#endif #endif
/* the standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
* and y and invert y */
new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
/* flip axes and negate value for y */
new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
if (_mag_topic != -1) { if (_mag_topic != -1) {
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
struct mag_report report; struct mag_report report;
ssize_t sz; ssize_t sz;
int ret = 1; int ret = 1;
uint8_t good_count = 0;
// XXX do something smarter here // XXX do something smarter here
int fd = (int)enable; int fd = (int)enable;
@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
1.0f, 1.0f,
}; };
float avg_excited[3] = {0.0f, 0.0f, 0.0f}; float sum_excited[3] = {0.0f, 0.0f, 0.0f};
unsigned i;
warnx("starting mag scale calibration"); /* expected axis scaling. The datasheet says that 766 will
* be places in the X and Y axes and 713 in the Z
* axis. Experiments show that in fact 766 is placed in X,
* and 713 in Y and Z. This is relative to a base of 660
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
/* do a simple demand read */ warnx("starting mag scale calibration");
sz = read(filp, (char *)&report, sizeof(report));
if (sz != sizeof(report)) {
warn("immediate read failed");
ret = 1;
goto out;
}
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
/* don't do this for now, it can lead to a crash in start() respectively work_queue() */
// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
// warn("failed to set queue depth");
// ret = 1;
// goto out;
// }
/* start the sensor polling at 50 Hz */ /* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out; goto out;
} }
/* Set to 2.5 Gauss */ /* Set to 2.5 Gauss. We ask for 3 to get the right part of
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { * the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range"); warnx("failed to set 2.5 Ga range");
ret = 1; ret = 1;
goto out; goto out;
@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out; goto out;
} }
/* read the sensor 10x and report each value */ // discard 10 samples to let the sensor settle
for (i = 0; i < 500; i++) { for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds; struct pollfd fds;
/* wait for data to be ready */ /* wait for data to be ready */
@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
if (sz != sizeof(report)) { if (sz != sizeof(report)) {
warn("periodic read failed"); warn("periodic read failed");
ret = -EIO;
goto out; goto out;
}
}
} else { /* read the sensor up to 50x, stopping when we have 10 good values */
avg_excited[0] += report.x; for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
avg_excited[1] += report.y; struct pollfd fds;
avg_excited[2] += report.z;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
good_count++;
sum_excited[0] += cal[0];
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
} }
//warnx("periodic read %u", i); //warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
} }
avg_excited[0] /= i; if (good_count < 5) {
avg_excited[1] /= i; warn("failed calibration");
avg_excited[2] /= i; ret = -EIO;
goto out;
}
warnx("done. Performed %u reads", i); #if 0
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3]; float scaling[3];
/* calculate axis scaling */ scaling[0] = sum_excited[0] / good_count;
scaling[0] = fabsf(1.16f / avg_excited[0]); scaling[1] = sum_excited[1] / good_count;
/* second axis inverted */ scaling[2] = sum_excited[2] / good_count;
scaling[1] = fabsf(1.16f / -avg_excited[1]);
scaling[2] = fabsf(1.08f / avg_excited[2]);
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]); warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
conf_reg &= ~0x03; conf_reg &= ~0x03;
} }
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
ret = write_reg(ADDR_CONF_A, conf_reg); ret = write_reg(ADDR_CONF_A, conf_reg);
if (OK != ret) if (OK != ret)
@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
uint8_t conf_reg_ret; uint8_t conf_reg_ret;
read_reg(ADDR_CONF_A, conf_reg_ret); read_reg(ADDR_CONF_A, conf_reg_ret);
//print_info();
return !(conf_reg == conf_reg_ret); return !(conf_reg == conf_reg_ret);
} }
@ -1211,6 +1221,10 @@ HMC5883::print_info()
perf_print_counter(_comms_errors); perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows); perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks); printf("poll interval: %u ticks\n", _measure_ticks);
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
_reports->print_info("report queue"); _reports->print_info("report queue");
} }

35
src/drivers/px4fmu/fmu.cpp

@ -1006,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break; break;
case PWM_SERVO_SET_COUNT: {
/* change the number of outputs that are enabled for
* PWM. This is used to change the split between GPIO
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
* FMUv1
*/
switch (arg) {
case 0:
set_mode(MODE_NONE);
break;
case 2:
set_mode(MODE_2PWM);
break;
case 4:
set_mode(MODE_4PWM);
break;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
case 6:
set_mode(MODE_6PWM);
break;
#endif
default:
ret = -EINVAL;
break;
}
break;
}
case MIXERIOCRESET: case MIXERIOCRESET:
if (_mixers != nullptr) { if (_mixers != nullptr) {
delete _mixers; delete _mixers;
@ -1443,7 +1477,6 @@ void
sensor_reset(int ms) sensor_reset(int ms)
{ {
int fd; int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR); fd = open(PX4FMU_DEVICE_PATH, O_RDWR);

49
src/drivers/px4io/px4io.cpp

@ -1454,8 +1454,10 @@ PX4IO::io_publish_raw_rc()
/* set RSSI */ /* set RSSI */
// XXX the correct scaling needs to be validated here if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) {
rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; // 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 */ /* lazily advertise on first publication */
if (_to_input_rc == 0) { if (_to_input_rc == 0) {
@ -1808,7 +1810,7 @@ PX4IO::print_status()
printf("\n"); 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); 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); 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 */ /* create the driver - it will set g_dev */
(void)new PX4IO(interface); (void)new PX4IO(interface);
if (g_dev == nullptr) if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed"); errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) { if (OK != g_dev->init()) {
delete g_dev; delete g_dev;
@ -2642,17 +2646,17 @@ monitor(void)
read(0, &c, 1); read(0, &c, 1);
if (cancels-- == 0) { 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); exit(0);
} }
} }
if (g_dev != nullptr) { 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_status();
(void)g_dev->print_debug(); (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 { } else {
errx(1, "driver not loaded, exiting"); errx(1, "driver not loaded, exiting");
@ -2767,18 +2771,33 @@ px4io_main(int argc, char *argv[])
} }
if (g_dev == nullptr) { if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade"); warnx("px4io is not started, still attempting upgrade");
} else {
uint16_t arg = atol(argv[2]); /* allocate the interface */
int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); device::Device *interface = get_interface();
if (ret != OK) {
printf("reboot failed - %d\n", ret); /* create the driver - it will set g_dev */
exit(1); (void)new PX4IO(interface);
if (g_dev == nullptr) {
delete interface;
errx(1, "driver alloc failed");
} }
// tear down the px4io instance if (OK != g_dev->init()) {
delete g_dev; 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 // upload the specified firmware
const char *fn[2]; const char *fn[2];
fn[0] = argv[3]; fn[0] = argv[3];

53
src/modules/px4iofirmware/adc.c

@ -83,6 +83,14 @@ adc_init(void)
{ {
adc_perf = perf_alloc(PC_ELAPSED, "adc"); 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 */ /* do calibration if supported */
#ifdef ADC_CR2_CAL #ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL; rCR2 |= ADC_CR2_RSTCAL;
@ -96,41 +104,25 @@ adc_init(void)
if (rCR2 & ADC_CR2_CAL) if (rCR2 & ADC_CR2_CAL)
return -1; return -1;
#endif #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; rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */ rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
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
/* configure for a single-channel sequence */ /* configure for a single-channel sequence */
rSQR1 = 0; rSQR1 = 0;
rSQR2 = 0; rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */ rSQR3 = 0; /* will be updated with the channel at conversion time */
/* 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);
return 0; return 0;
} }
@ -141,11 +133,12 @@ adc_init(void)
uint16_t uint16_t
adc_measure(unsigned channel) adc_measure(unsigned channel)
{ {
perf_begin(adc_perf); perf_begin(adc_perf);
/* clear any previous EOC */ /* clear any previous EOC */
if (rSR & ADC_SR_EOC) rSR = 0;
rSR &= ~ADC_SR_EOC; (void)rDR;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel; rSQR3 = channel;
@ -158,7 +151,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */ /* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) { if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
perf_end(adc_perf); perf_end(adc_perf);
return 0xffff; return 0xffff;
} }
@ -166,6 +158,7 @@ adc_measure(unsigned channel)
/* read the result and clear EOC */ /* read the result and clear EOC */
uint16_t result = rDR; uint16_t result = rDR;
rSR = 0;
perf_end(adc_perf); perf_end(adc_perf);
return result; return result;

11
src/modules/px4iofirmware/controls.c

@ -114,9 +114,20 @@ controls_tick() {
perf_begin(c_gather_sbus); 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_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) { if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; 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); perf_end(c_gather_sbus);
/* /*

55
src/modules/px4iofirmware/px4io.c

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
POWER_SERVO(true); POWER_SERVO(true);
#endif #endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */ /* start the safety switch handler */
safety_init(); safety_init();
@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */ /* initialise the control inputs */
controls_init(); controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */ /* start the FMU interface */
interface_init(); interface_init();
@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
/* initialize PWM limit lib */ /* initialize PWM limit lib */
pwm_limit_init(&pwm_limit); pwm_limit_init(&pwm_limit);
#if 0 /*
/* not enough memory, lock down */ * P O L I C E L I G H T S
if (minfo.mxordblk < 500) { *
* 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"); lowsyslog("ERR: not enough MEM");
bool phase = false; bool phase = false;
if (phase) { while (true) {
LED_AMBER(true);
LED_BLUE(false); if (phase) {
} else { LED_AMBER(true);
LED_AMBER(false); LED_BLUE(false);
LED_BLUE(true); } else {
} LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase; phase = !phase;
usleep(300000); }
} }
#endif
/* Start the failsafe led init */
failsafe_led_init();
/* /*
* Run everything in a tight loop. * Run everything in a tight loop.
@ -270,7 +295,6 @@ user_start(int argc, char *argv[])
check_reboot(); check_reboot();
#if 0
/* check for debug activity */ /* check for debug activity */
show_debug_messages(); show_debug_messages();
@ -287,7 +311,6 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk); (unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time(); last_debug_time = hrt_absolute_time();
} }
#endif
} }
} }

2
src/modules/px4iofirmware/px4io.h

@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit;
# define PX4IO_RELAY_CHANNELS 0 # define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) # 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)) # 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. * Safety switch/LED.
*/ */
extern void safety_init(void); extern void safety_init(void);
extern void failsafe_led_init(void);
/** /**
* FMU communications * FMU communications

8
src/modules/px4iofirmware/safety.c

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

Loading…
Cancel
Save