fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 05:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'fmuv2_bringup' into fmuv2_bringup_io2

Browse Source
This commit is contained in:
px4dev
2013-05-22 21:39:30 +02:00
parent e67022f874 78d29045c4
commit 437d9e4180
364 changed files with 23971 additions and 121563 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/px4iofirmware/adc.c
+7 -3
View File
@@ -135,6 +135,9 @@ adc_init(void)
return 0;
}
/*
return one measurement, or 0xffff on error
*/
uint16_t
adc_measure(unsigned channel)
{
@@ -154,9 +157,10 @@ adc_measure(unsigned channel)
while (!(rSR & ADC_SR_EOC)) {
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 1000) {
if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
break;
perf_end(adc_perf);
return 0xffff;
}
}
@@ -165,4 +169,4 @@ adc_measure(unsigned channel)
perf_end(adc_perf);
return result;
}
}
src/modules/px4iofirmware/controls.c
+1 -2
View File
@@ -217,9 +217,8 @@ controls_tick() {
if (assigned_channels == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag and clear RC lost alarm */
/* set RC OK flag */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/*
src/modules/px4iofirmware/mixer.cpp
+2 -3
View File
@@ -100,7 +100,6 @@ mixer_tick(void)
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
}
source = MIX_FAILSAFE;
@@ -174,7 +173,7 @@ mixer_tick(void)
* here.
*/
bool should_arm = (
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@@ -246,7 +245,7 @@ void
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
return;
}
src/modules/px4iofirmware/protocol.h
+3 -5
View File
@@ -138,7 +138,7 @@
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -168,9 +168,9 @@
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
#define PX4IO_P_SETUP_ARMING_FMU_ARMED (1 << 1) /* FMU is already armed */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
@@ -178,8 +178,6 @@
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */
#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
src/modules/px4iofirmware/registers.c
+21 -24
View File
@@ -138,15 +138,14 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
[PX4IO_P_SETUP_IBATT_SCALE] = 0,
[PX4IO_P_SETUP_IBATT_BIAS] = 0,
[PX4IO_P_SETUP_SET_DEBUG] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -204,7 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
break;
@@ -313,7 +311,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
}
@@ -364,7 +362,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_PAGE_RC_CONFIG: {
/* do not allow a RC config change while fully armed */
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
break;
}
@@ -414,18 +412,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
count++;
}
/* assert deadzone is sane */
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
count++;
}
// The following check isn't needed as constraint checks in controls.c will catch this.
//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
// count++;
//}
//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
// count++;
//}
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
count++;
}
@@ -508,20 +498,27 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
if (counts != 0xffff) {
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
}
/* PX4IO_P_STATUS_IBATT */
{
unsigned counts = adc_measure(ADC_VBATT);
unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
if (corrected < 0)
corrected = 0;
r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
/*
note that we have no idea what sort of
current sensor is attached, so we just
return the raw 12 bit ADC value and let the
FMU sort it out, with user selectable
configuration for their sensor
*/
unsigned counts = adc_measure(ADC_IN5);
if (counts != 0xffff) {
r_page_status[PX4IO_P_STATUS_IBATT] = counts;
}
}
SELECT_PAGE(r_page_status);
src/modules/px4iofirmware/safety.c
+15 -15
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013 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
@@ -32,7 +32,8 @@
****************************************************************************/
/**
* @file Safety button logic.
* @file safety.c
* Safety button logic.
*/
#include <nuttx/config.h>
@@ -56,11 +57,11 @@ static unsigned counter = 0;
/*
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_SAFE 0xffff /**< always on */
#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */
#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
static unsigned blink_counter = 0;
@@ -109,7 +110,8 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
@@ -120,8 +122,6 @@ safety_check_button(void *arg)
counter++;
}
/* Disarm quickly */
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
@@ -138,21 +138,21 @@ safety_check_button(void *arg)
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
pattern = LED_PATTERN_FMU_OK_TO_ARM;
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */