Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Lorenz Meier
2013-10-30 09:15:55 +01:00
parent 34d2f318ac 0fa03e65ab
commit dc80d6745e
75 changed files with 2891 additions and 824 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/fixed_wing_control.odg
BIN
View File
Binary file not shown.
Documentation/l1_control.odg
BIN
View File
Binary file not shown.
ROMFS/px4fmu_common/init.d/1000_rc_fw.hil → ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
View File
ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
+110
View File
@@ -0,0 +1,110 @@
#!nsh
#
# USB HIL start
#
echo "[HIL] HILS quadrotor + starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.0
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.05
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 3.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.1
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.05
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.5
param set MPC_THR_MIN 0.1
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# 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 commander (depends on orb, mavlink)
#
commander start
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
#
# Start position estimator
#
position_estimator_inav start
#
# Start attitude control
#
multirotor_att_control start
#
# Start position control
#
multirotor_pos_control start
echo "[HIL] setup done, running"
ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
+103
View File
@@ -0,0 +1,103 @@
#!nsh
#
# USB HIL start
#
echo "[HIL] HILStar starting.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set FW_L1_PERIOD 16
param set RC_SCALE_ROLL 1.0
param set RC_SCALE_PITCH 1.0
param set SYS_AUTOCONFIG 0
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# 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 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
set MODE autostart
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
else
echo "Using /etc/mixers/FMU_AERT.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
fi
fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"
ROMFS/px4fmu_common/init.d/12-13_hex
+95
View File
@@ -0,0 +1,95 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Hex"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.004
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.12
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
param set MC_YAWRATE_D 0.005
param set MC_YAWRATE_I 0.2
param set MC_YAWRATE_P 0.3
param set NAV_TAKEOFF_ALT 3.0
param set MPC_TILT_MAX 0.5
param set MPC_THR_MAX 0.7
param set MPC_THR_MIN 0.3
param set MPC_XY_D 0
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
# 13 = hexarotor
#
param set MAV_TYPE 13
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900 900 900
px4io min 1200 1200 1200 1200 1200 1200
px4io max 1900 1900 1900 1900 1900 1900
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Load mixer
#
mixer load /dev/pwm_output $MIXER
#
# Set PWM output frequency to 400 Hz
#
pwm -u 400 -m 0xff
#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/32_skywalker_x5
+86
View File
@@ -0,0 +1,86 @@
#!nsh
echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5"
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
# TODO
param set SYS_AUTOCONFIG 0
param save
fi
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing
#
param set MAV_TYPE 1
set EXIT_ON_END no
#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
px4io limit 100
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
commander start
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the sensors and test them.
#
sh /etc/init.d/rc.sensors
#
# Start logging (depends on sensors)
#
sh /etc/init.d/rc.logging
#
# Start GPS interface (depends on orb)
#
gps start
#
# Start the attitude and position estimator
#
att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
then
echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
else
echo "Using /etc/mixers/FMU_Q.mix"
mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
fi
fw_att_control start
fw_pos_control_l1 start
if [ $EXIT_ON_END == yes ]
then
exit
fi
ROMFS/px4fmu_common/init.d/40_io_segway
+1 -1
View File
@@ -52,5 +52,5 @@ attitude_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
md25 start 3 0x58
roboclaw start /dev/ttyS2 128 1200
segway start
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
+2 -2
View File
@@ -58,7 +58,7 @@ usleep 10000
if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start -d /dev/ttyS1 -b 115200
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
commander start
@@ -67,7 +67,7 @@ then
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS1 -b 115200
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
ROMFS/px4fmu_common/init.d/rcS
+59 -17
View File
@@ -108,25 +108,41 @@ then
if param compare SYS_AUTOSTART 1000
then
sh /etc/init.d/1000_rc_fw.hil
sh /etc/init.d/1000_rc_fw_easystar.hil
set MODE custom
else
if param compare SYS_AUTOSTART 1001
then
sh /etc/init.d/1001_rc_quad.hil
set MODE custom
else
if param compare SYS_AUTOSTART 1002
then
sh /etc/init.d/1002_rc_fw_state.hil
set MODE custom
else
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
fi
fi
if param compare SYS_AUTOSTART 1001
then
sh /etc/init.d/1001_rc_quad.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1002
then
sh /etc/init.d/1002_rc_fw_state.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1003
then
sh /etc/init.d/1003_rc_quad_+.hil
set MODE custom
fi
if param compare SYS_AUTOSTART 1004
then
sh /etc/init.d/1004_rc_fw_Rascal110.hil
set MODE custom
fi
if [ $MODE != custom ]
then
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
#
# Upgrade PX4IO firmware
#
@@ -177,6 +193,20 @@ then
sh /etc/init.d/11_dji_f450
set MODE custom
fi
if param compare SYS_AUTOSTART 12
then
set MIXER /etc/mixers/FMU_hex_x.mix
sh /etc/init.d/12-13_hex
set MODE custom
fi
if param compare SYS_AUTOSTART 13
then
set MIXER /etc/mixers/FMU_hex_+.mix
sh /etc/init.d/12-13_hex
set MODE custom
fi
if param compare SYS_AUTOSTART 15
then
@@ -248,13 +278,25 @@ then
sh /etc/init.d/30_io_camflyer
set MODE custom
fi
if param compare SYS_AUTOSTART 31
then
sh /etc/init.d/31_io_phantom
set MODE custom
fi
if param compare SYS_AUTOSTART 32
then
sh /etc/init.d/32_skywalker_x5
set MODE custom
fi
if param compare SYS_AUTOSTART 40
then
sh /etc/init.d/40_io_segway
set MODE custom
fi
if param compare SYS_AUTOSTART 100
then
sh /etc/init.d/100_mpx_easystar
ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+20
View File
@@ -62,3 +62,23 @@ range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_AET.mix
+20
View File
@@ -58,3 +58,23 @@ range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_Q.mix
+18
View File
@@ -50,3 +50,21 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_RET.mix
+20
View File
@@ -51,3 +51,23 @@ range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_X5.mix
+19
View File
@@ -48,3 +48,22 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_delta.mix
+20
View File
@@ -48,3 +48,23 @@ M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000
Gimbal / flaps / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+11
View File
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co
are mixed 100%.
R: 6+ 10000 10000 10000 0
Gimbal / payload mixer for last two channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+11
View File
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration. All c
are mixed 100%.
R: 6x 10000 10000 10000 0
Gimbal / payload mixer for last two channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
+20
View File
@@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con
are mixed 100%.
R: 4+ 10000 10000 10000 0
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
+19
View File
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration. All co
are mixed 100%.
R: 4v 10000 10000 10000 0
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
+19
View File
@@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU
This file defines a single mixer for a quadrotor with a wide configuration. All controls are mixed 100%.
R: 4w 10000 10000 10000 0
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
+19
View File
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration. All co
are mixed 100%.
R: 4x 10000 10000 10000 0
Gimbal / payload mixer for last four channels
-----------------------------------------------------
M: 1
O: 10000 10000 0 -10000 10000
S: 0 4 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 5 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 6 10000 10000 0 -10000 10000
M: 1
O: 10000 10000 0 -10000 10000
S: 0 7 10000 10000 0 -10000 10000
Tools/px4params/.gitignore
+2
View File
@@ -0,0 +1,2 @@
parameters.wiki
parameters.xml
Tools/sdlog2_dump.py
+12 -13
View File
@@ -25,8 +25,12 @@ import struct, sys
if sys.hexversion >= 0x030000F0:
runningPython3 = True
def _parseCString(cstr):
return str(cstr, 'ascii').split('\0')[0]
else:
runningPython3 = False
def _parseCString(cstr):
return str(cstr).split('\0')[0]
class SDLog2Parser:
BLOCK_SIZE = 8192
@@ -175,9 +179,9 @@ class SDLog2Parser:
self.__csv_columns.append(full_label)
self.__csv_data[full_label] = None
if self.__file != None:
print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
else:
print(self.__csv_delim.join(self.__csv_columns))
print(self.__csv_delim.join(self.__csv_columns))
def __printCSVRow(self):
s = []
@@ -190,9 +194,9 @@ class SDLog2Parser:
s.append(v)
if self.__file != None:
print(self.__csv_delim.join(s), file=self.__file)
print(self.__csv_delim.join(s), file=self.__file)
else:
print(self.__csv_delim.join(s))
print(self.__csv_delim.join(s))
def __parseMsgDescr(self):
if runningPython3:
@@ -202,14 +206,9 @@ class SDLog2Parser:
msg_type = data[0]
if msg_type != self.MSG_TYPE_FORMAT:
msg_length = data[1]
if runningPython3:
msg_name = str(data[2], 'ascii').strip("\0")
msg_format = str(data[3], 'ascii').strip("\0")
msg_labels = str(data[4], 'ascii').strip("\0").split(",")
else:
msg_name = str(data[2]).strip("\0")
msg_format = str(data[3]).strip("\0")
msg_labels = str(data[4]).strip("\0").split(",")
msg_name = _parseCString(data[2])
msg_format = _parseCString(data[3])
msg_labels = _parseCString(data[4]).split(",")
# Convert msg_format to struct.unpack format string
msg_struct = ""
msg_mults = []
@@ -243,7 +242,7 @@ class SDLog2Parser:
data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
for i in range(len(data)):
if type(data[i]) is str:
data[i] = data[i].strip("\0")
data[i] = _parseCString(data[i])
m = msg_mults[i]
if m != None:
data[i] = data[i] * m
makefiles/config_px4fmu-v1_default.mk
+3 -2
View File
@@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/rgbled
MODULES += drivers/mkblctrl
MODULES += drivers/md25
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
@@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX needs state machine update
MODULES += modules/segway
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
@@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
#
# Demo apps
makefiles/config_px4fmu-v2_default.mk
+3 -1
View File
@@ -31,6 +31,7 @@ MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry
MODULES += drivers/hott/hott_sensors
MODULES += drivers/blinkm
MODULES += drivers/roboclaw
MODULES += drivers/airspeed
MODULES += drivers/ets_airspeed
MODULES += drivers/meas_airspeed
@@ -77,7 +78,7 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
#MODULES += modules/segway # XXX needs state machine update
MODULES += modules/segway
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
@@ -111,6 +112,7 @@ MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
#
# Demo apps
src/drivers/drv_accel.h
+4 -2
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Accelerometer driver interface.
* @file drv_accel.h
*
* Accelerometer driver interface.
*/
#ifndef _DRV_ACCEL_H
@@ -66,7 +68,7 @@ struct accel_report {
int16_t temperature_raw;
};
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
struct accel_scale {
float x_offset;
float x_scale;
src/drivers/drv_airspeed.h
+4 -1
View File
@@ -32,7 +32,10 @@
****************************************************************************/
/**
* @file Airspeed driver interface.
* @file drv_airspeed.h
*
* Airspeed driver interface.
*
* @author Simon Wilks
*/
src/drivers/drv_baro.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Barometric pressure sensor driver interface.
* @file drv_baro.h
*
* Barometric pressure sensor driver interface.
*/
#ifndef _DRV_BARO_H
src/drivers/drv_gps.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file GPS driver interface.
* @file drv_gps.h
*
* GPS driver interface.
*/
#ifndef _DRV_GPS_H
src/drivers/drv_gyro.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Gyroscope driver interface.
* @file drv_gyro.h
*
* Gyroscope driver interface.
*/
#ifndef _DRV_GYRO_H
src/drivers/drv_rc_input.h
+2 -1
View File
@@ -32,8 +32,9 @@
****************************************************************************/
/**
* @file R/C input interface.
* @file drv_rc_input.h
*
* R/C input interface.
*/
#ifndef _DRV_RC_INPUT_H
src/drivers/drv_sensor.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Common sensor API and ioctl definitions.
* @file drv_sensor.h
*
* Common sensor API and ioctl definitions.
*/
#ifndef _DRV_SENSOR_H
src/drivers/drv_tone_alarm.h
+3 -1
View File
@@ -31,7 +31,9 @@
*
****************************************************************************/
/*
/**
* @file drv_tone_alarm.h
*
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* The tone_alarm driver supports a set of predefined "alarm"
src/drivers/ets_airspeed/ets_airspeed.cpp
-3
View File
@@ -132,11 +132,8 @@ ETSAirspeed::measure()
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
src/drivers/lsm303d/lsm303d.cpp
+29 -3
View File
@@ -241,11 +241,18 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg7_expected;
uint8_t _reg1_expected;
/**
* Start automatic measurement.
*/
@@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
{
// enable debug() calls
_debug_enabled = true;
@@ -526,10 +537,12 @@ void
LSM303D::reset()
{
/* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
}
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
return OK;
}
@@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
@@ -1282,6 +1302,12 @@ LSM303D::measure()
void
LSM303D::mag_measure()
{
if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
perf_count(_reg7_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
src/drivers/meas_airspeed/meas_airspeed.cpp
-3
View File
@@ -138,11 +138,8 @@ MEASAirspeed::measure()
if (OK != ret) {
perf_count(_comms_errors);
return ret;
}
ret = OK;
return ret;
}
src/drivers/px4io/px4io_serial.cpp
+17 -11
View File
@@ -48,6 +48,7 @@
#include <time.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
@@ -262,7 +263,8 @@ PX4IO_serial::init()
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
/* rCR3 = USART_CR3_EIE;*/
rCR3 = USART_CR3_EIE;
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* create semaphores */
@@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete()
DMA_SCR_PBURST_SINGLE |
DMA_SCR_MBURST_SINGLE);
stm32_dmastart(_tx_dma, nullptr, nullptr, false);
//rCR1 &= ~USART_CR1_TE;
//rCR1 |= USART_CR1_TE;
rCR3 |= USART_CR3_DMAT;
perf_end(_pc_dmasetup);
/* compute the deadline for a 5ms timeout */
/* compute the deadline for a 10ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
#if 0
abstime.tv_sec++; /* long timeout for testing */
#else
abstime.tv_nsec += 10000000; /* 0ms timeout */
if (abstime.tv_nsec > 1000000000) {
abstime.tv_nsec += 10*1000*1000;
if (abstime.tv_nsec >= 1000*1000*1000) {
abstime.tv_sec++;
abstime.tv_nsec -= 1000000000;
abstime.tv_nsec -= 1000*1000*1000;
}
#endif
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
int ret;
@@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt()
*/
if (_rx_dma_status == _dma_status_waiting) {
_abort_dma();
perf_count(_pc_uerrs);
perf_count(_pc_uerrs);
/* complete DMA as though in error */
_do_rx_dma_callback(DMA_STATUS_TEIF);
@@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt()
unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);
/* stop the receive DMA */
stm32_dmastop(_rx_dma);
/* complete the short reception */
_do_rx_dma_callback(DMA_STATUS_TEIF);
return;
}
@@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma()
stm32_dmastop(_rx_dma);
}
#endif /* PX4IO_SERIAL_BASE */
#endif /* PX4IO_SERIAL_BASE */
src/drivers/rgbled/rgbled.cpp
+24 -3
View File
@@ -172,7 +172,20 @@ RGBLED::probe()
bool on, powersave;
uint8_t r, g, b;
ret = get(on, powersave, r, g, b);
/**
this may look strange, but is needed. There is a serial
EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
a bunch of I2C addresses, including the 0x55 used by this
LED device. So we need to do enough operations to be sure
we are talking to the right device. These 3 operations seem
to be enough, as the 3rd one consistently fails if no
RGBLED is on the bus.
*/
if ((ret=get(on, powersave, r, g, b)) != OK ||
(ret=send_led_enable(false) != OK) ||
(ret=send_led_enable(false) != OK)) {
return ret;
}
return ret;
}
@@ -561,7 +574,7 @@ rgbled_main(int argc, char *argv[])
int ch;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
rgbledadr = strtol(optarg, NULL, 0);
@@ -577,7 +590,12 @@ rgbled_main(int argc, char *argv[])
}
}
const char *verb = argv[1];
if (optind >= argc) {
rgbled_usage();
exit(1);
}
const char *verb = argv[optind];
int fd;
int ret;
@@ -598,6 +616,9 @@ rgbled_main(int argc, char *argv[])
if (g_rgbled == nullptr) {
// fall back to default bus
if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
errx(1, "init failed");
}
i2cdevice = PX4_I2C_BUS_LED;
}
}
src/drivers/roboclaw/RoboClaw.cpp
+328
View File
@@ -0,0 +1,328 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file RoboClaw.cpp
*
* RoboClaw Motor Driver
*
* references:
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
*
*/
#include "RoboClaw.hpp"
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h>
#include <systemlib/err.h>
#include <arch/board/board.h>
#include <mavlink/mavlink_log.h>
#include <controllib/uorb/UOrbPublication.hpp>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>
uint8_t RoboClaw::checksum_mask = 0x7f;
RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
uint16_t pulsesPerRev):
_address(address),
_pulsesPerRev(pulsesPerRev),
_uart(0),
_controlPoll(),
_actuators(NULL, ORB_ID(actuator_controls_0), 20),
_motor1Position(0),
_motor1Speed(0),
_motor1Overflow(0),
_motor2Position(0),
_motor2Speed(0),
_motor2Overflow(0)
{
// setup control polling
_controlPoll.fd = _actuators.getHandle();
_controlPoll.events = POLLIN;
// start serial port
_uart = open(deviceName, O_RDWR | O_NOCTTY);
if (_uart < 0) err(1, "could not open %s", deviceName);
int ret = 0;
struct termios uart_config;
ret = tcgetattr(_uart, &uart_config);
if (ret < 0) err (1, "failed to get attr");
uart_config.c_oflag &= ~ONLCR; // no CR for every LF
ret = cfsetispeed(&uart_config, B38400);
if (ret < 0) err (1, "failed to set input speed");
ret = cfsetospeed(&uart_config, B38400);
if (ret < 0) err (1, "failed to set output speed");
ret = tcsetattr(_uart, TCSANOW, &uart_config);
if (ret < 0) err (1, "failed to set attr");
// setup default settings, reset encoders
resetEncoders();
}
RoboClaw::~RoboClaw()
{
setMotorDutyCycle(MOTOR_1, 0.0);
setMotorDutyCycle(MOTOR_2, 0.0);
close(_uart);
}
int RoboClaw::readEncoder(e_motor motor)
{
uint16_t sum = 0;
if (motor == MOTOR_1) {
_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
} else if (motor == MOTOR_2) {
_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
}
uint8_t rbuf[50];
usleep(5000);
int nread = read(_uart, rbuf, 50);
if (nread < 6) {
printf("failed to read\n");
return -1;
}
//printf("received: \n");
//for (int i=0;i<nread;i++) {
//printf("%d\t", rbuf[i]);
//}
//printf("\n");
uint32_t count = 0;
uint8_t * countBytes = (uint8_t *)(&count);
countBytes[3] = rbuf[0];
countBytes[2] = rbuf[1];
countBytes[1] = rbuf[2];
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5];
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask;
// check if checksum is valid
if (checksum != checksum_computed) {
printf("checksum failed: expected %d got %d\n",
checksum, checksum_computed);
return -1;
}
int overFlow = 0;
if (status & STATUS_REVERSE) {
//printf("roboclaw: reverse\n");
}
if (status & STATUS_UNDERFLOW) {
//printf("roboclaw: underflow\n");
overFlow = -1;
} else if (status & STATUS_OVERFLOW) {
//printf("roboclaw: overflow\n");
overFlow = +1;
}
static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) {
_motor1Overflow += overFlow;
_motor1Position = float(int64_t(count) +
_motor1Overflow*overflowAmount)/_pulsesPerRev;
} else if (motor == MOTOR_2) {
_motor2Overflow += overFlow;
_motor2Position = float(int64_t(count) +
_motor2Overflow*overflowAmount)/_pulsesPerRev;
}
return 0;
}
void RoboClaw::printStatus(char *string, size_t n)
{
snprintf(string, n,
"pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
double(getMotorPosition(MOTOR_1)),
double(getMotorSpeed(MOTOR_1)),
double(getMotorPosition(MOTOR_2)),
double(getMotorSpeed(MOTOR_2)));
}
float RoboClaw::getMotorPosition(e_motor motor)
{
if (motor == MOTOR_1) {
return _motor1Position;
} else if (motor == MOTOR_2) {
return _motor2Position;
}
}
float RoboClaw::getMotorSpeed(e_motor motor)
{
if (motor == MOTOR_1) {
return _motor1Speed;
} else if (motor == MOTOR_2) {
return _motor2Speed;
}
}
int RoboClaw::setMotorSpeed(e_motor motor, float value)
{
uint16_t sum = 0;
// bound
if (value > 1) value = 1;
if (value < -1) value = -1;
uint8_t speed = fabs(value)*127;
// send command
if (motor == MOTOR_1) {
if (value > 0) {
return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
} else {
return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
}
} else if (motor == MOTOR_2) {
if (value > 0) {
return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
} else {
return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
}
}
return -1;
}
int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
{
uint16_t sum = 0;
// bound
if (value > 1) value = 1;
if (value < -1) value = -1;
int16_t duty = 1500*value;
// send command
if (motor == MOTOR_1) {
return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
(uint8_t *)(&duty), 2, sum);
} else if (motor == MOTOR_2) {
return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
(uint8_t *)(&duty), 2, sum);
}
return -1;
}
int RoboClaw::resetEncoders()
{
uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS,
nullptr, 0, sum);
}
int RoboClaw::update()
{
// wait for an actuator publication,
// check for exit condition every second
// note "::poll" is required to distinguish global
// poll from member function for driver
if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
// if new data, send to motors
if (_actuators.updated()) {
_actuators.update();
setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
}
return 0;
}
uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
{
uint16_t sum = 0;
//printf("sum\n");
for (size_t i=0;i<n;i++) {
sum += buf[i];
//printf("%d\t", buf[i]);
}
//printf("total sum %d\n", sum);
return sum;
}
int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
size_t n_data, uint16_t & prev_sum)
{
tcflush(_uart, TCIOFLUSH); // flush buffers
uint8_t buf[n_data + 3];
buf[0] = _address;
buf[1] = cmd;
for (size_t i=0;i<n_data;i++) {
buf[i+2] = data[n_data - i - 1]; // MSB
}
uint16_t sum = _sumBytes(buf, n_data + 2);
prev_sum += sum;
buf[n_data + 2] = sum & checksum_mask;
//printf("\nmessage:\n");
//for (size_t i=0;i<n_data+3;i++) {
//printf("%d\t", buf[i]);
//}
//printf("\n");
return write(_uart, buf, n_data + 3);
}
int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev)
{
printf("roboclaw test: starting\n");
// setup
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
char buf[200];
for (int i=0; i<10; i++) {
usleep(100000);
roboclaw.readEncoder(RoboClaw::MOTOR_1);
roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.printStatus(buf,200);
printf("%s", buf);
}
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
for (int i=0; i<10; i++) {
usleep(100000);
roboclaw.readEncoder(RoboClaw::MOTOR_1);
roboclaw.readEncoder(RoboClaw::MOTOR_2);
roboclaw.printStatus(buf,200);
printf("%s", buf);
}
printf("Test complete\n");
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/roboclaw/RoboClaw.hpp
+192
View File
@@ -0,0 +1,192 @@
/****************************************************************************
*
* Copyright (C) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file RoboClas.hpp
*
* RoboClaw Motor Driver
*
* references:
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
*
*/
#pragma once
#include <poll.h>
#include <stdio.h>
#include <controllib/uorb/UOrbSubscription.hpp>
#include <uORB/topics/actuator_controls.h>
#include <drivers/device/i2c.h>
/**
* This is a driver for the RoboClaw motor controller
*/
class RoboClaw
{
public:
/** control channels */
enum e_channel {
CH_VOLTAGE_LEFT = 0,
CH_VOLTAGE_RIGHT
};
/** motors */
enum e_motor {
MOTOR_1 = 0,
MOTOR_2
};
/**
* constructor
* @param deviceName the name of the
* serial port e.g. "/dev/ttyS2"
* @param address the adddress of the motor
* (selectable on roboclaw)
* @param pulsesPerRev # of encoder
* pulses per revolution of wheel
*/
RoboClaw(const char *deviceName, uint16_t address,
uint16_t pulsesPerRev);
/**
* deconstructor
*/
virtual ~RoboClaw();
/**
* @return position of a motor, rev
*/
float getMotorPosition(e_motor motor);
/**
* @return speed of a motor, rev/sec
*/
float getMotorSpeed(e_motor motor);
/**
* set the speed of a motor, rev/sec
*/
int setMotorSpeed(e_motor motor, float value);
/**
* set the duty cycle of a motor, rev/sec
*/
int setMotorDutyCycle(e_motor motor, float value);
/**
* reset the encoders
* @return status
*/
int resetEncoders();
/**
* main update loop that updates RoboClaw motor
* dutycycle based on actuator publication
*/
int update();
/**
* read data from serial
*/
int readEncoder(e_motor motor);
/**
* print status
*/
void printStatus(char *string, size_t n);
private:
// Quadrature status flags
enum e_quadrature_status_flags {
STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
};
// commands
// We just list the commands we want from the manual here.
enum e_command {
// simple
CMD_DRIVE_FWD_1 = 0,
CMD_DRIVE_REV_1 = 1,
CMD_DRIVE_FWD_2 = 4,
CMD_DRIVE_REV_2 = 5,
// encoder commands
CMD_READ_ENCODER_1 = 16,
CMD_READ_ENCODER_2 = 17,
CMD_RESET_ENCODERS = 20,
// advanced motor control
CMD_READ_SPEED_HIRES_1 = 30,
CMD_READ_SPEED_HIRES_2 = 31,
CMD_SIGNED_DUTYCYCLE_1 = 32,
CMD_SIGNED_DUTYCYCLE_2 = 33,
};
static uint8_t checksum_mask;
uint16_t _address;
uint16_t _pulsesPerRev;
int _uart;
/** poll structure for control packets */
struct pollfd _controlPoll;
/** actuator controls subscription */
control::UOrbSubscription<actuator_controls_s> _actuators;
// private data
float _motor1Position;
float _motor1Speed;
int16_t _motor1Overflow;
float _motor2Position;
float _motor2Speed;
int16_t _motor2Overflow;
// private methods
uint16_t _sumBytes(uint8_t * buf, size_t n);
int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum);
};
// unit testing
int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev);
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/drivers/roboclaw/module.mk
+41
View File
@@ -0,0 +1,41 @@
############################################################################
#
# Copyright (c) 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# RoboClaw Motor Controller
#
MODULE_COMMAND = roboclaw
SRCS = roboclaw_main.cpp \
RoboClaw.cpp
src/drivers/roboclaw/roboclaw_main.cpp
+195
View File
@@ -0,0 +1,195 @@
/****************************************************************************
*
* Copyright (c) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @ file roboclaw_main.cpp
*
* RoboClaw Motor Driver
*
* references:
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
*
*/
#include <nuttx/config.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <arch/board/board.h>
#include "RoboClaw.hpp"
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
/**
* Deamon management function.
*/
extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int roboclaw_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage();
static void usage()
{
fprintf(stderr, "usage: roboclaw "
"{start|stop|status|test}\n\n");
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int roboclaw_main(int argc, char *argv[])
{
if (argc < 1)
usage();
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("roboclaw already running\n");
/* this is not an error */
exit(0);
}
thread_should_exit = false;
deamon_task = task_spawn_cmd("roboclaw",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2048,
roboclaw_thread_main,
(const char **)argv);
exit(0);
} else if (!strcmp(argv[1], "test")) {
const char * deviceName = "/dev/ttyS2";
uint8_t address = 128;
uint16_t pulsesPerRev = 1200;
if (argc == 2) {
printf("testing with default settings\n");
} else if (argc != 4) {
printf("usage: roboclaw test device address pulses_per_rev\n");
exit(-1);
} else {
deviceName = argv[2];
address = strtoul(argv[3], nullptr, 0);
pulsesPerRev = strtoul(argv[4], nullptr, 0);
}
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
deviceName, address, pulsesPerRev);
roboclawTest(deviceName, address, pulsesPerRev);
thread_should_exit = true;
exit(0);
} else if (!strcmp(argv[1], "stop")) {
thread_should_exit = true;
exit(0);
} else if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\troboclaw app is running\n");
} else {
printf("\troboclaw app not started\n");
}
exit(0);
}
usage();
exit(1);
}
int roboclaw_thread_main(int argc, char *argv[])
{
printf("[roboclaw] starting\n");
// skip parent process args
argc -=2;
argv +=2;
if (argc < 3) {
printf("usage: roboclaw start device address\n");
return -1;
}
const char *deviceName = argv[1];
uint8_t address = strtoul(argv[2], nullptr, 0);
uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
deviceName, address, pulsesPerRev);
// start
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
thread_running = true;
// loop
while (!thread_should_exit) {
roboclaw.update();
}
// exit
printf("[roboclaw] exiting.\n");
thread_running = false;
return 0;
}
// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
src/lib/conversion/module.mk
+38
View File
@@ -0,0 +1,38 @@
############################################################################
#
# Copyright (C) 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
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Conversion library
#
SRCS = rotation.cpp
src/lib/conversion/rotation.cpp
+62
View File
@@ -0,0 +1,62 @@
/****************************************************************************
*
* Copyright (C) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rotation.cpp
*
* Vector rotation library
*/
#include "math.h"
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
{
/* first set to zero */
rot_matrix->Matrix::zero(3, 3);
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
math::EulerAngles euler(roll, pitch, yaw);
math::Dcm R(euler);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
(*rot_matrix)(i, j) = R(i, j);
}
}
}
src/lib/conversion/rotation.h
+121
View File
@@ -0,0 +1,121 @@
/****************************************************************************
*
* Copyright (C) 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rotation.h
*
* Vector rotation library
*/
#ifndef ROTATION_H_
#define ROTATION_H_
#include <unistd.h>
#include <mathlib/mathlib.h>
/**
* Enum for board and external compass rotations.
* This enum maps from board attitude to airframe attitude.
*/
enum Rotation {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
ROTATION_YAW_135 = 3,
ROTATION_YAW_180 = 4,
ROTATION_YAW_225 = 5,
ROTATION_YAW_270 = 6,
ROTATION_YAW_315 = 7,
ROTATION_ROLL_180 = 8,
ROTATION_ROLL_180_YAW_45 = 9,
ROTATION_ROLL_180_YAW_90 = 10,
ROTATION_ROLL_180_YAW_135 = 11,
ROTATION_PITCH_180 = 12,
ROTATION_ROLL_180_YAW_225 = 13,
ROTATION_ROLL_180_YAW_270 = 14,
ROTATION_ROLL_180_YAW_315 = 15,
ROTATION_ROLL_90 = 16,
ROTATION_ROLL_90_YAW_45 = 17,
ROTATION_ROLL_90_YAW_90 = 18,
ROTATION_ROLL_90_YAW_135 = 19,
ROTATION_ROLL_270 = 20,
ROTATION_ROLL_270_YAW_45 = 21,
ROTATION_ROLL_270_YAW_90 = 22,
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_MAX
};
typedef struct {
uint16_t roll;
uint16_t pitch;
uint16_t yaw;
} rot_lookup_t;
const rot_lookup_t rot_lookup[] = {
{ 0, 0, 0 },
{ 0, 0, 45 },
{ 0, 0, 90 },
{ 0, 0, 135 },
{ 0, 0, 180 },
{ 0, 0, 225 },
{ 0, 0, 270 },
{ 0, 0, 315 },
{180, 0, 0 },
{180, 0, 45 },
{180, 0, 90 },
{180, 0, 135 },
{ 0, 180, 0 },
{180, 0, 225 },
{180, 0, 270 },
{180, 0, 315 },
{ 90, 0, 0 },
{ 90, 0, 45 },
{ 90, 0, 90 },
{ 90, 0, 135 },
{270, 0, 0 },
{270, 0, 45 },
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
};
/**
* Get the rotation matrix
*/
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
#endif /* ROTATION_H_ */
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+2 -2
View File
@@ -43,7 +43,7 @@
float ECL_L1_Pos_Controller::nav_roll()
{
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
return ret;
}
@@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float xtrackErr = vector_A_to_airplane % vector_AB;
float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
/* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
src/lib/ecl/l1/ecl_l1_pos_controller.h
+11
View File
@@ -222,6 +222,15 @@ public:
_K_L1 = 4.0f * _L1_damping * _L1_damping;
}
/**
* Set the maximum roll angle output in radians
*
*/
void set_l1_roll_limit(float roll_lim_rad) {
_roll_lim_rad = roll_lim_rad;
}
private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
@@ -238,6 +247,8 @@ private:
float _K_L1; ///< L1 control gain for _L1_damping
float _heading_omega; ///< Normalized frequency
float _roll_lim_rad; ///<maximum roll angle
/**
* Convert a 2D vector from WGS84 to planar coordinates.
*
src/lib/external_lgpl/tecs/tecs.h
+5
View File
@@ -94,6 +94,11 @@ public:
// Rate of change of velocity along X body axis in m/s^2
float get_VXdot(void) { return _vel_dot; }
float get_speed_weight() {
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
src/modules/commander/accelerometer_calibration.cpp
+211 -135
View File
File diff suppressed because it is too large Load Diff

Some files were not shown because too many files have changed in this diff Show More