aboutsummaryrefslogtreecommitdiff
path: root/ROMFS/px4fmu_common/init.d/09_ardrone_flow
blob: e7173f6e6efb8fd0deb0a6c7b2f191c8ff297589 (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
#!nsh
#
# Flight startup script for PX4FMU on PX4IOAR carrier board.
#

# Disable the USB interface
set USB no

# Disable autostarting other apps
set MODE ardrone
 
echo "[init] doing PX4IOAR startup..."
 
#
# Start the ORB
#
uorb start
 
#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
	param load /fs/microsd/params
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

#
# Configure PX4FMU for operation with PX4IOAR
#
fmu mode_gpio_serial
 
#
# Start the sensors.
#
sh /etc/init.d/rc.sensors

#
# Start MAVLink and MAVLink Onboard (Flow Sensor)
#
mavlink start -d /dev/ttyS0 -b 57600
mavlink_onboard start -d /dev/ttyS3 -b 115200
usleep 5000

#
# Start the commander.
#
commander start

#
# Start the attitude estimator
#
attitude_estimator_ekf start

#
# Start the position estimator
#
flow_position_estimator start

#
# Fire up the multi rotor attitude controller
#
multirotor_att_control start

#
# Fire up the flow position controller
#
flow_position_control start

#
# Fire up the flow speed controller
#
flow_speed_control start

#
# Fire up the AR.Drone interface.
#
ardrone_interface start -d /dev/ttyS1
 
#
# startup is done; we don't want the shell because we
# use the same UART for telemetry
#
echo "[init] startup done"

exit