Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge branch 'px4io-i2c' into px4io-i2c-nuttx

Browse Source
This commit is contained in:
Lorenz Meier
2013-02-17 18:13:09 +01:00
parent 5085a28240 985838d971
commit 219279ac82
8 changed files with 174 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/scripts/rc.FMU_quad_x
+55 -28
View File
@@ -1,40 +1,67 @@
#!nsh
#
# Flight startup script for PX4FMU with PWM outputs.
#
# Disable the USB interface
set USB no
# Disable autostarting other apps
set MODE custom
echo "[init] doing PX4FMU Quad startup..."
#
# Start the ORB
#
# Startup for X-quad on FMU1.5/1.6
#
echo "[init] uORB"
uorb start
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
then
param load
param load /fs/microsd/parameters
fi
echo "[init] sensors"
#bma180 start
#l3gd20 start
mpu6000 start
hmc5883 start
ms5611 start
sensors start
echo "[init] mavlink"
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
echo "[init] commander"
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start the commander.
#
commander start
echo "[init] attitude control"
#
# Start the attitude estimator
#
attitude_estimator_ekf start
multirotor_att_control start
echo "[init] starting PWM output"
fmu mode_pwm
mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Start attitude control
#
multirotor_att_control start
echo "[init] startup done, exiting"
exit
exit
ROMFS/scripts/rc.PX4IO
+52 -45
View File
@@ -1,73 +1,80 @@
#!nsh
# Disable USB and autostart
set USB no
set MODE camflyer
#
# Start the object request broker
# Start the ORB
#
uorb start
#
# Init the EEPROM
# Load microSD params
#
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
echo "[init] loading microSD params"
param select /fs/microsd/parameters
if [ -f /fs/microsd/parameters ]
then
param load
param load /fs/microsd/parameters
fi
#
# Enable / connect to PX4IO
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
px4io start
#
# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
# which is good for testing. See ROMFS/mixers for a full list of mixers.
#
mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
param set MAV_TYPE 1
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
#
# Start the commander.
#
commander start
#
# Start GPS interface
#
gps start
#
# Start the attitude estimator
#
attitude_estimator_ekf start
kalman_demo start
#
# Start PX4IO interface
#
px4io start
#
# Load mixer and start controllers
#
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
control_demo start
#
# Start logging
#
sdlog start -s 10
#
# Start the attitude and position controller
# Start system state
#
fixedwing_att_control start
fixedwing_pos_control start
#
# Start GPS capture. Comment this out if you do not have a GPS.
#
gps start
#
# Start logging to microSD if we can
#
sh /etc/init.d/rc.logging
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry
#
echo "[init] startup done"
exit
if blinkm start
then
echo "using BlinkM for state indication"
blinkm systemstate
else
echo "no BlinkM found, OK."
fi
ROMFS/scripts/rc.PX4IOAR
+29 -11
View File
@@ -17,7 +17,7 @@ echo "[init] doing PX4IOAR startup..."
uorb start
#
# Load microSD params
# Init the parameter storage
#
echo "[init] loading microSD params"
param select /fs/microsd/parameters
@@ -26,17 +26,24 @@ then
param load /fs/microsd/parameters
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 2
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors
#
# Start MAVLink
#
mavlink start -d /dev/ttyS0 -b 57600
usleep 5000
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start the commander.
#
@@ -62,15 +69,26 @@ multirotor_att_control start
#
ardrone_interface start -d /dev/ttyS1
#
# Start logging
#
#sdlog start
#
# Start GPS capture
#
gps start
#
# Start logging
#
sdlog start -s 10
#
# Start system state
#
if blinkm start
then
echo "using BlinkM for state indication"
blinkm systemstate
else
echo "no BlinkM found, OK."
fi
#
# startup is done; we don't want the shell because we
apps/commander/commander.c
-2
View File
@@ -1467,8 +1467,6 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_system_id, &(current_status.system_id));
param_get(_param_component_id, &(current_status.component_id));
} else {
warnx("ARMED, rejecting sys type change\n");
}
}
apps/drivers/px4io/px4io.cpp
+19 -8
View File
@@ -279,6 +279,8 @@ PX4IO::PX4IO() :
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
_status(0),
_alarms(0),
_task(-1),
_task_should_exit(false),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
@@ -360,8 +362,8 @@ PX4IO::init()
uint16_t reg;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &reg, sizeof(reg));
/* get IO's last seen FMU state */
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
if (ret != OK)
return ret;
@@ -392,8 +394,8 @@ PX4IO::init()
/* wait 10 ms */
usleep(10000);
/* abort after 10s */
if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
log("failed to recover from in-air restart (1), aborting IO driver init.");
return 1;
}
@@ -432,8 +434,8 @@ PX4IO::init()
/* wait 10 ms */
usleep(10000);
/* abort after 10s */
if ((hrt_absolute_time() - try_start_time)/1000 > 10000) {
/* abort after 5s */
if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
log("failed to recover from in-air restart (2), aborting IO driver init.");
return 1;
}
@@ -770,19 +772,28 @@ PX4IO::io_handle_status(uint16_t status)
*/
/* check for IO reset - force it back to armed if necessary */
if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
&& !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the arming flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED);
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
_status &= PX4IO_P_STATUS_FLAGS_ARMED;
} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
/* set the sync flag */
ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
/* set new status */
_status = status;
} else {
ret = 0;
/* set new status */
_status = status;
}
return ret;
}
apps/px4io/protocol.h
+1
View File
@@ -102,6 +102,7 @@
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
apps/px4io/px4io.c
+8 -8
View File
@@ -84,7 +84,7 @@ static volatile uint8_t msg_next_out, msg_next_in;
* output.
*/
#define NUM_MSG 2
static char msg[NUM_MSG][40];
static char msg[NUM_MSG][50];
/*
add a debug message to be printed on the console
@@ -183,23 +183,23 @@ int user_start(int argc, char *argv[])
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* run the mixer at 100Hz (for now...) */
/* run the mixer at ~300Hz (for now...) */
/* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
uint8_t counter=0;
uint16_t counter=0;
for (;;) {
/*
if we are not scheduled for 100ms then reset the I2C bus
if we are not scheduled for 10ms then reset the I2C bus
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);
hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL);
poll(NULL, 0, 10);
poll(NULL, 0, 3);
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
show_debug_messages();
if (counter++ == 200) {
if (counter++ == 800) {
counter = 0;
isr_debug(1, "tick dbg=%u stat=0x%x arm=0x%x feat=0x%x rst=%u",
isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u",
(unsigned)debug_level,
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
apps/px4io/registers.c
+10
View File
@@ -270,6 +270,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_status_alarms &= ~value;
break;
case PX4IO_P_STATUS_FLAGS:
/*
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
break;
default:
/* just ignore writes to other registers in this page */
break;