forked from Archive/PX4-Autopilot
Merged master
This commit is contained in:
commit
985838d971
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue