fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier 2013-05-14 22:47:21 +02:00
commit d02f5c5505
14 changed files with 91 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ROMFS/px4fmu_common/init.d/rc.FMU_quad_x
View File

@ -20,10 +20,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.IO_QUAD
View File

@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.PX4IO
View File

@ -13,10 +13,10 @@ uorb start
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

8
ROMFS/px4fmu_common/init.d/rc.PX4IOAR
View File

@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Init the parameter storage
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
ROMFS/px4fmu_common/init.d/rc.hil
View File

@ -17,10 +17,10 @@ hil mode_pwm
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
param load /fs/microsd/parameters
param load /fs/microsd/params
fi
#

6
makefiles/firmware.mk
View File

@ -201,9 +201,9 @@ MODULES := $(sort $(MODULES))
# locate the first instance of a module by full path or by looking on the
# module search path
define MODULE_SEARCH
$(abspath $(firstword $(wildcard $(1)/module.mk) \
$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
MISSING_$1))
$(firstword $(abspath $(wildcard $(1)/module.mk)) \
$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
MISSING_$1)
endef
# make a list of module makefiles and check that we found them all

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

@ -416,7 +416,7 @@ PX4IO::init()
* already armed, and has been configured for in-air restart
*/
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
@ -500,10 +500,9 @@ PX4IO::init()
/* dis-arm IO before touching anything */
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
PX4IO_P_SETUP_ARMING_ARM_OK |
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
/* publish RC config to IO */
ret = io_set_rc_config();
@ -702,16 +701,18 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_ARM_OK;
if (armed.armed && !armed.lockdown) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
if (vstatus.flag_vector_flight_mode_ok) {
set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (vstatus.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
@ -1277,9 +1278,9 @@ PX4IO::print_status()
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
_battery_amp_per_volt,
_battery_amp_bias,
_battery_mamphour_total);
(double)_battery_amp_per_volt,
(double)_battery_amp_bias,
(double)_battery_mamphour_total);
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@ -1311,9 +1312,9 @@ PX4IO::print_status()
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@ -1354,12 +1355,12 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
switch (cmd) {
case PWM_SERVO_ARM:
/* set the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
break;
case PWM_SERVO_DISARM:
/* clear the 'armed' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:

30
src/modules/commander/commander.c
View File

@ -1503,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
current_status.state_machine == SYSTEM_STATE_AUTO ||
current_status.state_machine == SYSTEM_STATE_MANUAL)) {
/* armed */
led_toggle(LED_BLUE);
/* armed, solid */
led_on(LED_AMBER);
} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* not armed */
led_toggle(LED_BLUE);
led_toggle(LED_AMBER);
}
/* toggle error led at 5 Hz in HIL mode */
if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
/* GPS lock */
led_on(LED_BLUE);
} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* no GPS lock, but GPS module is aquiring lock */
led_toggle(LED_BLUE);
}
} else {
/* no GPS info, don't light the blue led */
led_off(LED_BLUE);
}
/* toggle GPS led at 5 Hz in HIL mode */
if (current_status.flag_hil_enabled) {
/* hil enabled */
led_toggle(LED_AMBER);
led_toggle(LED_BLUE);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
/* toggle error (red) at 5 Hz on low battery or error */
/* toggle arming (red) at 5 Hz on low battery or error */
led_toggle(LED_AMBER);
} else {

5
src/modules/commander/state_machine_helper.c
View File

@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
{
struct actuator_armed_s armed;
armed.armed = current_status->flag_system_armed;
/* XXX allow arming by external components on multicopters only if not yet armed by RC */
/* XXX allow arming only if core sensors are ok */
armed.ready_to_arm = true;
/* lock down actuators if required, only in HIL */
armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);

4
src/modules/px4iofirmware/mixer.cpp
View File

@ -174,7 +174,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 +246,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;
}

4
src/modules/px4iofirmware/protocol.h
View File

@ -145,9 +145,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 */

9
src/modules/px4iofirmware/registers.c
View File

@ -142,9 +142,10 @@ volatile uint16_t r_page_setup[] =
};
#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)
@ -311,7 +312,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;
}
@ -362,7 +363,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;
}

30
src/modules/px4iofirmware/safety.c
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 */

1
src/modules/uORB/topics/actuator_controls.h
View File

@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
/** global 'actuator output is live' control. */
struct actuator_armed_s {
bool armed; /**< Set to true if system is armed */
bool ready_to_arm; /**< Set to true if system is ready to be armed */
bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
};