diff options
417 files changed, 18558 insertions, 5151 deletions
diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 803d96453..4c539b345 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -56,7 +56,7 @@ define vecstate if $mmfsr & (1<<3) printf " during exception return" end - if $mmfsr & (1<<0) + if $mmfsr & (1<<1) printf " during data access" end if $mmfsr & (1<<0) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index b0864a229..093edd0e0 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -2,6 +2,106 @@ import gdb, gdb.types +class NX_register_set(object): + """Copy of the registers for a given context""" + + v7_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 11, + 'R1': 12, + 'R2': 13, + 'R3': 14, + 'R12': 15, + 'R14': 16, + 'LR': 16, + 'R15': 17, + 'PC': 17, + 'XPSR': 18, + } + + v7em_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 27, + 'R1': 28, + 'R2': 29, + 'R3': 30, + 'R12': 31, + 'R14': 32, + 'LR': 32, + 'R15': 33, + 'PC': 33, + 'XPSR': 34, + } + + regs = dict() + + def __init__(self, xcpt_regs): + if xcpt_regs is None: + self.regs['R0'] = long(gdb.parse_and_eval('$r0')) + self.regs['R1'] = long(gdb.parse_and_eval('$r1')) + self.regs['R2'] = long(gdb.parse_and_eval('$r2')) + self.regs['R3'] = long(gdb.parse_and_eval('$r3')) + self.regs['R4'] = long(gdb.parse_and_eval('$r4')) + self.regs['R5'] = long(gdb.parse_and_eval('$r5')) + self.regs['R6'] = long(gdb.parse_and_eval('$r6')) + self.regs['R7'] = long(gdb.parse_and_eval('$r7')) + self.regs['R8'] = long(gdb.parse_and_eval('$r8')) + self.regs['R9'] = long(gdb.parse_and_eval('$r9')) + self.regs['R10'] = long(gdb.parse_and_eval('$r10')) + self.regs['R11'] = long(gdb.parse_and_eval('$r11')) + self.regs['R12'] = long(gdb.parse_and_eval('$r12')) + self.regs['R13'] = long(gdb.parse_and_eval('$r13')) + self.regs['SP'] = long(gdb.parse_and_eval('$sp')) + self.regs['R14'] = long(gdb.parse_and_eval('$r14')) + self.regs['LR'] = long(gdb.parse_and_eval('$lr')) + self.regs['R15'] = long(gdb.parse_and_eval('$r15')) + self.regs['PC'] = long(gdb.parse_and_eval('$pc')) + self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + else: + for key in self.v7em_regmap.keys(): + self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + + + @classmethod + def with_xcpt_regs(cls, xcpt_regs): + return cls(xcpt_regs) + + @classmethod + def for_current(cls): + return cls(None) + + def __format__(self, format_spec): + return format_spec.format( + registers = self.registers + ) + + @property + def registers(self): + return self.regs + + class NX_task(object): """Reference to a NuttX task and methods for introspecting it""" @@ -139,56 +239,12 @@ class NX_task(object): if 'registers' not in self.__dict__: registers = dict() if self._state_is('TSTATE_TASK_RUNNING'): - # XXX need to fix this to handle interrupt context - registers['R0'] = long(gdb.parse_and_eval('$r0')) - registers['R1'] = long(gdb.parse_and_eval('$r1')) - registers['R2'] = long(gdb.parse_and_eval('$r2')) - registers['R3'] = long(gdb.parse_and_eval('$r3')) - registers['R4'] = long(gdb.parse_and_eval('$r4')) - registers['R5'] = long(gdb.parse_and_eval('$r5')) - registers['R6'] = long(gdb.parse_and_eval('$r6')) - registers['R7'] = long(gdb.parse_and_eval('$r7')) - registers['R8'] = long(gdb.parse_and_eval('$r8')) - registers['R9'] = long(gdb.parse_and_eval('$r9')) - registers['R10'] = long(gdb.parse_and_eval('$r10')) - registers['R11'] = long(gdb.parse_and_eval('$r11')) - registers['R12'] = long(gdb.parse_and_eval('$r12')) - registers['R13'] = long(gdb.parse_and_eval('$r13')) - registers['SP'] = long(gdb.parse_and_eval('$sp')) - registers['R14'] = long(gdb.parse_and_eval('$r14')) - registers['LR'] = long(gdb.parse_and_eval('$lr')) - registers['R15'] = long(gdb.parse_and_eval('$r15')) - registers['PC'] = long(gdb.parse_and_eval('$pc')) - registers['XPSR'] = long(gdb.parse_and_eval('$xpsr')) - # this would only be valid if we were in an interrupt - registers['EXC_RETURN'] = 0 - # we should be able to get this... - registers['PRIMASK'] = 0 + registers = NX_register_set.for_current().registers else: context = self._tcb['xcp'] regs = context['regs'] - registers['R0'] = long(regs[27]) - registers['R1'] = long(regs[28]) - registers['R2'] = long(regs[29]) - registers['R3'] = long(regs[30]) - registers['R4'] = long(regs[2]) - registers['R5'] = long(regs[3]) - registers['R6'] = long(regs[4]) - registers['R7'] = long(regs[5]) - registers['R8'] = long(regs[6]) - registers['R9'] = long(regs[7]) - registers['R10'] = long(regs[8]) - registers['R11'] = long(regs[9]) - registers['R12'] = long(regs[31]) - registers['R13'] = long(regs[0]) - registers['SP'] = long(regs[0]) - registers['R14'] = long(regs[32]) - registers['LR'] = long(regs[32]) - registers['R15'] = long(regs[33]) - registers['PC'] = long(regs[33]) - registers['XPSR'] = long(regs[34]) - registers['EXC_RETURN'] = long(regs[10]) - registers['PRIMASK'] = long(regs[1]) + registers = NX_register_set.with_xcpt_regs(regs).registers + self.__dict__['registers'] = registers return self.__dict__['registers'] @@ -267,8 +323,7 @@ class NX_show_heap (gdb.Command): def _print_allocations(self, region_start, region_end): if region_start >= region_end: - print 'heap region {} corrupt'.format(hex(region_start)) - return + raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start print 'heap {} - {}'.format(region_start, region_end) cursor = 1 @@ -286,13 +341,31 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print "{} heap(s)".format(nregions) + print '{} heap(s)'.format(nregions) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) NX_show_heap() +class NX_show_interrupted_thread (gdb.Command): + """(NuttX) prints the register state of an interrupted thread when in interrupt/exception context""" + def __init__(self): + super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER) + def invoke(self, args, from_tty): + regs = gdb.lookup_global_symbol('current_regs').value() + if regs is 0: + raise gdb.GdbError('not in interrupt context') + else: + registers = NX_register_set.with_xcpt_regs(regs) + my_fmt = '' + my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n' + my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n' + my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' + my_fmt += ' R12 {registers[PC]:#010x}\n' + my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' + print format(registers, my_fmt) +NX_show_interrupted_thread() diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg Binary files differnew file mode 100644 index 000000000..0918edcac --- /dev/null +++ b/Documentation/fixed_wing_control.odg @@ -40,6 +40,17 @@ export PX4_BASE := $(realpath $(dir $(lastword $(MAKEFILE_LIST))))/ include $(PX4_BASE)makefiles/setup.mk # +# Get a version string provided by git +# This assumes that git command is available and that +# the directory holding this file also contains .git directory +# +GIT_DESC := $(shell git log -1 --pretty=format:%H) +ifneq ($(words $(GIT_DESC)),1) + GIT_DESC := "unknown_git_version" +endif +export GIT_DESC + +# # Canned firmware configurations that we (know how to) build. # KNOWN_CONFIGS := $(subst config_,,$(basename $(notdir $(wildcard $(PX4_MK_DIR)config_*.mk)))) diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index f6d82a5ec..f6f09ac22 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -8,7 +8,18 @@ echo "[init] 08_ardrone: PX4FMU on PX4IOAR carrier board" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save @@ -19,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 794342a0b..9b739f245 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -8,7 +8,18 @@ echo "[init] 09_ardrone_flow: PX4FMU on PX4IOAR carrier board with PX4FLOW" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set MC_ATTRATE_D 0 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_P 0.13 + param set MC_ATT_D 0 + param set MC_ATT_I 0.3 + param set MC_ATT_P 5 + param set MC_YAWPOS_D 0.1 + param set MC_YAWPOS_I 0.15 + param set MC_YAWPOS_P 1 + param set MC_YAWRATE_D 0 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_P 0.15 param set SYS_AUTOCONFIG 0 param save @@ -19,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil new file mode 100644 index 000000000..5e4028bbb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -0,0 +1,92 @@ +#!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) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil new file mode 100644 index 000000000..bbe3b7e28 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -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_x.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" + diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil new file mode 100644 index 000000000..d5782a89b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -0,0 +1,84 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILStar starting in state-HIL mode.." + +# +# 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 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 + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +fw_pos_control_l1 start +fw_att_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil new file mode 100644 index 000000000..1c85e04f2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -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" + diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil new file mode 100644 index 000000000..8c5e4b6a8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -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" + diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index e1cefdb99..4f843e9aa 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -1,6 +1,6 @@ #!nsh -echo "[init] Multiplex Easystar" +echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" # # Load default params for this platform @@ -8,7 +8,29 @@ echo "[init] Multiplex Easystar" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + 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 @@ -20,28 +42,34 @@ fi # param set MAV_TYPE 1 -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +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 # -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 - -# -# Start the commander -# -commander start - -# -# Start the sensors +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -49,9 +77,9 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + # -# Start GPS interface +# Start GPS interface (depends on orb) # gps start @@ -63,6 +91,18 @@ att_pos_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] +then + echo "Using FMU_RET mixer from sd card" + mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix +else + echo "Using standard FMU_RET mixer" + mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix +fi fw_att_control start fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler new file mode 100644 index 000000000..cef86c34d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -0,0 +1,101 @@ +#!nsh + +echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" + +# +# 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 + +# +# 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) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix +fw_att_control start +fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330 index b8fe5fc31..81ea292aa 100644 --- a/ROMFS/px4fmu_common/init.d/10_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/10_dji_f330 @@ -59,11 +59,10 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -81,7 +80,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450 index 388f40a47..4dbf76cee 100644 --- a/ROMFS/px4fmu_common/init.d/11_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/11_dji_f450 @@ -43,11 +43,9 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex new file mode 100644 index 000000000..0f0bb05ce --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12-13_hex @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery index cbfde6d3c..bd6189a6d 100644 --- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery @@ -43,11 +43,9 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 @@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals (for DJI ESCs) +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris index b6c5fbdea..d8cc0e913 100644 --- a/ROMFS/px4fmu_common/init.d/16_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris @@ -10,12 +10,12 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.006 + param set MC_ATTRATE_D 0.004 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_P 0.13 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 5.0 + param set MC_ATT_P 9.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.15 param set MC_YAWPOS_P 0.5 @@ -43,17 +43,15 @@ then mavlink start usleep 5000 + commander start + sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs - px4io idle 900 900 900 900 - px4io min 1200 1200 1200 1200 - px4io max 1800 1800 1800 1800 else # Start MAVLink (on UART1 / ttyS0) mavlink start -d /dev/ttyS0 usleep 5000 fmu mode_pwm - param set BAT_V_SCALING 0.004593 + param set BAT_V_SCALING 0.0098 set EXIT_ON_END yes fi @@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer index f7e653362..191d8cd95 100644 --- a/ROMFS/px4fmu_common/init.d/30_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer @@ -1,6 +1,6 @@ #!nsh -echo "[init] 30_io_camflyer: PX4FMU+PX4IO on Camflyer" +echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" # # Load default params for this platform @@ -19,34 +19,35 @@ fi # MAV_TYPE 1 = fixed wing # param set MAV_TYPE 1 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start the commander (depends on orb, mavlink) -# -commander start - -# -# Start PX4IO interface (depends on orb, commander) + +set EXIT_ON_END no + # -px4io start - +# Start and configure PX4IO or FMU interface # -# Allow PX4IO to recover from midair restarts. -# this is very unlikely, but quite safe and robust. -px4io recovery +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 + 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 (depends on orb, px4io) +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -54,7 +55,7 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + # # Start GPS interface (depends on orb) # @@ -71,3 +72,8 @@ att_pos_estimator_ekf start mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index e1e609927..652833745 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -1,6 +1,6 @@ #!nsh -echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" +echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" # # Load default params for this platform @@ -8,7 +8,29 @@ echo "[init] 31_io_phantom: PX4FMU+PX4IO on Phantom" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + 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 17 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 param set SYS_AUTOCONFIG 0 param save @@ -20,28 +42,34 @@ fi # param set MAV_TYPE 1 -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +set EXIT_ON_END no # -# Start and configure PX4IO interface +# Start and configure PX4IO or FMU interface # -sh /etc/init.d/rc.io +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start -# -# Set actuator limit to 100 Hz update (50 Hz PWM) -px4io limit 100 - -# -# Start the commander -# -commander 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 +# Start the sensors and test them. # sh /etc/init.d/rc.sensors @@ -49,9 +77,9 @@ sh /etc/init.d/rc.sensors # Start logging (depends on sensors) # sh /etc/init.d/rc.logging - + # -# Start GPS interface +# Start GPS interface (depends on orb) # gps start @@ -66,3 +94,8 @@ att_pos_estimator_ekf start mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix fw_att_control start fw_pos_control_l1 start + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 new file mode 100644 index 000000000..cd7677112 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5 @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 4b7ed5286..fb9dec043 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 index ae41f2a01..eae37098b 100644 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 @@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix # # Set PWM output frequency # -pwm -u 400 -m 0xff +pwm rate -c 1234 -r 400 # # Start common for all multirotors apps diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test new file mode 100644 index 000000000..f86f4f85b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/cmp_test @@ -0,0 +1,9 @@ +#!nsh + +cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +then + echo "CMP returned true" +else + echo "CMP returned false" +fi
\ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl new file mode 100644 index 000000000..a63d0e5f1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -0,0 +1,120 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + 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 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start the Mikrokopter ESC driver +# +if [ $MKBLCTRL_MODE == yes ] +then + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" + mkblctrl -mkmode x + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" + mkblctrl -mkmode + + fi +else + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" + fi + mkblctrl +fi + +usleep 10000 + +# +# 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 +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +if [ $MKBLCTRL_FRAME == x ] +then + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +fi + +# +# Set disarmed, min and max PWM signals +# +pwm disarmed -c 1234 -p 900 +pwm min -c 1234 -p 1200 +pwm max -c 1234 -p 1800 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc new file mode 100644 index 000000000..0c0cfa53d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc @@ -0,0 +1,122 @@ +#!nsh + +# +# 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.002 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_P 0.09 + param set MC_ATT_D 0.0 + param set MC_ATT_I 0.0 + param set MC_ATT_P 6.8 + 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 + +echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start -d /dev/ttyS1 -b 57600 + usleep 5000 + + commander start + + sh /etc/init.d/rc.io +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS1 -b 57600 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +if [ $ESC_MAKER = afro ] +then + # Set PWM values for Afro ESCs + pwm disarmed -c 1234 -p 1050 + pwm min -c 1234 -p 1080 + pwm max -c 1234 -p 1860 +else + # Set PWM values for typical ESCs + pwm disarmed -c 1234 -p 900 + pwm min -c 1234 -p 980 + pwm max -c 1234 -p 1800 +fi + +# +# Load mixer +# +if [ $FRAME_GEOMETRY == x ] +then + echo "Frame geometry X" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + if [ $FRAME_GEOMETRY == w ] + then + echo "Frame geometry W" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix + else + echo "Frame geometry +" + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix + fi +fi + +# +# Set PWM output frequency +# +pwm rate -r 400 -c 1234 + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi + +echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil deleted file mode 100644 index eccb2b767..000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ /dev/null @@ -1,56 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] starting.." - -uorb start - -# Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/ttyS1 - -# 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) -# -mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index e3638e3d1..dfff07198 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -14,11 +14,6 @@ sh /etc/init.d/rc.sensors sh /etc/init.d/rc.logging # -# Start the commander. -# -commander start - -# # Start GPS interface (depends on orb) # gps start diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 2828a108b..f17b650bc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -27,6 +27,22 @@ else set BOARD fmuv2 fi +# Start airspeed sensors +if meas_airspeed start +then + echo "using MEAS airspeed sensor" +else + if ets_airspeed start + then + echo "using ETS airspeed sensor (bus 3)" + else + if ets_airspeed start -b 1 + then + echo "Using ETS airspeed sensor (bus 1)" + fi + fi +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9264985d6..ccf2cd47e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -21,7 +21,6 @@ if mavlink stop then echo "stopped other MAVLink instance" fi -sleep 2 mavlink start -b 230400 -d /dev/ttyACM0 # Stop commander @@ -37,13 +36,6 @@ then echo "Commander started" fi -# Stop px4io -if px4io stop -then - echo "PX4IO stopped" -fi -sleep 1 - # Start px4io if present if px4io start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 32fb67a45..cff8446a6 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -73,20 +73,25 @@ then # # Load microSD params # - if ramtron start - then - param select /ramtron/params - if [ -f /ramtron/params ] - then - param load /ramtron/params - fi - else + #if ramtron start + #then + # param select /ramtron/params + # if [ -f /ramtron/params ] + # then + # param load /ramtron/params + # fi + #else param select /fs/microsd/params if [ -f /fs/microsd/params ] then - param load /fs/microsd/params + if param load /fs/microsd/params + then + echo "Parameters loaded" + else + echo "Parameter file corrupt - ignoring" + fi fi - fi + #fi # # Start system state indicator @@ -101,8 +106,42 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1000 + then + sh /etc/init.d/1000_rc_fw_easystar.hil + set MODE custom + 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 @@ -154,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 @@ -166,19 +219,96 @@ then sh /etc/init.d/16_3dr_iris set MODE custom fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame + if param compare SYS_AUTOSTART 17 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame + if param compare SYS_AUTOSTART 18 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 19 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 20 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 21 + then + set FRAME_GEOMETRY x + set ESC_MAKER afro + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi + + # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame + if param compare SYS_AUTOSTART 22 + then + set FRAME_GEOMETRY w + sh /etc/init.d/rc.custom_io_esc + set MODE custom + fi if param compare SYS_AUTOSTART 30 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 + set MODE custom + fi + + if param compare SYS_AUTOSTART 101 + then + sh /etc/init.d/101_hk_bixler + set MODE custom + fi + # Start any custom extensions that might be missing if [ -f /fs/microsd/etc/rc.local ] then @@ -189,8 +319,9 @@ then if [ $MODE == autostart ] then # Telemetry port is on both FMU boards ttyS1 - mavlink start -b 57600 -d /dev/ttyS1 - usleep 5000 + # but the AR.Drone motors can be get 'flashed' + # if starting MAVLink on them - so do not + # start it as default (default link: USB) # Start commander commander start diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 75e82bb00..0ec663e35 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix index 20cb88b91..c73cb2a62 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix index ebcb66b24..17ff71151 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix index 95beb8927..f07c34ac8 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix index 9f81e1dc3..610868354 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix @@ -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 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix index 981466704..f0aa6650d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix @@ -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 + diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix index b5e38ce9e..f8f9f0e4d 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix index 8e8d122ad..26b40b9e9 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix index dfdf1d58e..cd9a9cfab 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix index 2a4a0f341..520aba635 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix index 81b4af30b..58e6af74b 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix @@ -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 diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix index 12a3bee20..fa21c8012 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix @@ -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 diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore new file mode 100644 index 000000000..73cf39575 --- /dev/null +++ b/Tools/px4params/.gitignore @@ -0,0 +1,2 @@ +parameters.wiki +parameters.xml
\ No newline at end of file diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md new file mode 100644 index 000000000..a23b44799 --- /dev/null +++ b/Tools/px4params/README.md @@ -0,0 +1,9 @@ +h1. PX4 Parameters Processor + +It's designed to scan PX4 source codes, find declarations of tunable parameters, +and generate the list in various formats. + +Currently supported formats are: + +* XML for the parametric UI generator +* Human-readable description in DokuWiki format diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py new file mode 100644 index 000000000..33f76b415 --- /dev/null +++ b/Tools/px4params/dokuwikiout.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py new file mode 100644 index 000000000..c09246871 --- /dev/null +++ b/Tools/px4params/output.py @@ -0,0 +1,5 @@ +class Output(object): + def Save(self, groups, fn): + data = self.Generate(groups) + with open(fn, 'w') as f: + f.write(data) diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py new file mode 100644 index 000000000..251be672f --- /dev/null +++ b/Tools/px4params/parser.py @@ -0,0 +1,220 @@ +import sys +import re + +class ParameterGroup(object): + """ + Single parameter group + """ + def __init__(self, name): + self.name = name + self.params = [] + + def AddParameter(self, param): + """ + Add parameter to the group + """ + self.params.append(param) + + def GetName(self): + """ + Get parameter group name + """ + return self.name + + def GetParams(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.params, + cmp=lambda x, y: cmp(x.GetFieldValue("code"), + y.GetFieldValue("code"))) + +class Parameter(object): + """ + Single parameter + """ + + # Define sorting order of the fields + priority = { + "code": 10, + "type": 9, + "short_desc": 8, + "long_desc": 7, + "default": 6, + "min": 5, + "max": 4, + # all others == 0 (sorted alphabetically) + } + + def __init__(self): + self.fields = {} + + def SetField(self, code, value): + """ + Set named field value + """ + self.fields[code] = value + + def GetFieldCodes(self): + """ + Return list of existing field codes in convenient order + """ + return sorted(self.fields.keys(), + cmp=lambda x, y: cmp(self.priority.get(y, 0), + self.priority.get(x, 0)) or cmp(x, y)) + + def GetFieldValue(self, code): + """ + Return value of the given field code or None if not found. + """ + return self.fields.get(code) + +class Parser(object): + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + + valid_tags = set(["min", "max", "group"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def GetSupportedExtensions(self): + """ + Returns list of supported file extensions that can be parsed by this + parser. + """ + return ["cpp", "c"] + + def Parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not starting with + # "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + # Non-empty line outside the comment + m = self.re_parameter_definition.match(line) + if m: + tp, code, defval = m.group(1, 2, 3) + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter() + param.SetField("code", code) + param.SetField("short_desc", code) + param.SetField("type", tp) + param.SetField("default", defval) + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + param.SetField("short_desc", + self.re_remove_dots.sub('', short_desc)) + if long_desc is not None: + param.SetField("long_desc", long_desc) + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid" + "documentation tag: '%s'\n" % tag) + else: + param.SetField(tag, tags[tag]) + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].AddParameter(param) + # Reset parsed comment. + state = None + + def GetParamGroups(self): + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + return sorted(self.param_groups.values(), + cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0), + self.priority.get(x.GetName(), 0)) or cmp(x.GetName(), + y.GetName())) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py new file mode 100755 index 000000000..cdf5aba7c --- /dev/null +++ b/Tools/px4params/px_process_params.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# It scans src/ subdirectory of the project, collects all parameters +# definitions, and outputs list of parameters in XML and DokuWiki formats. +# + +import scanner +import parser +import xmlout +import dokuwikiout + +# Initialize parser +prs = parser.Parser() + +# Scan directories, and parse the files +sc = scanner.Scanner() +sc.ScanDir("../../src", prs) +output = prs.GetParamGroups() + +# Output into XML +out = xmlout.XMLOutput() +out.Save(output, "parameters.xml") + +# Output into DokuWiki +out = dokuwikiout.DokuWikiOutput() +out.Save(output, "parameters.wiki") diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py new file mode 100644 index 000000000..b5a1af47c --- /dev/null +++ b/Tools/px4params/scanner.py @@ -0,0 +1,34 @@ +import os +import re + +class Scanner(object): + """ + Traverses directory tree, reads all source files, and passes their contents + to the Parser. + """ + + re_file_extension = re.compile(r'\.([^\.]+)$') + + def ScanDir(self, srcdir, parser): + """ + Scans provided path and passes all found contents to the parser using + parser.Parse method. + """ + extensions = set(parser.GetSupportedExtensions()) + for dirname, dirnames, filenames in os.walk(srcdir): + for filename in filenames: + m = self.re_file_extension.search(filename) + if m: + ext = m.group(1) + if ext in extensions: + path = os.path.join(dirname, filename) + self.ScanFile(path, parser) + + def ScanFile(self, path, parser): + """ + Scans provided file and passes its contents to the parser using + parser.Parse method. + """ + with open(path, 'r') as f: + contents = f.read() + parser.Parse(contents) diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py new file mode 100644 index 000000000..d56802b15 --- /dev/null +++ b/Tools/px4params/xmlout.py @@ -0,0 +1,22 @@ +import output +from xml.dom.minidom import getDOMImplementation + +class XMLOutput(output.Output): + def Generate(self, groups): + impl = getDOMImplementation() + xml_document = impl.createDocument(None, "parameters", None) + xml_parameters = xml_document.documentElement + for group in groups: + xml_group = xml_document.createElement("group") + xml_group.setAttribute("name", group.GetName()) + xml_parameters.appendChild(xml_group) + for param in group.GetParams(): + xml_param = xml_document.createElement("parameter") + xml_group.appendChild(xml_param) + for code in param.GetFieldCodes(): + value = param.GetFieldValue(code) + xml_field = xml_document.createElement(code) + xml_param.appendChild(xml_field) + xml_value = xml_document.createTextNode(value) + xml_field.appendChild(xml_value) + return xml_document.toprettyxml(indent=" ", newl="\n", encoding="utf-8") diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cab3c2fd0..a2d7d371d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -400,7 +400,19 @@ print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property( # Spin waiting for a device to show up while True: - for port in args.port.split(","): + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: #print("Trying %s" % port) @@ -418,7 +430,7 @@ while True: # Windows, don't open POSIX ports if not "/" in port: up = uploader(port, args.baud) - except: + except Exception: # open failed, rate-limit our attempts time.sleep(0.05) @@ -431,12 +443,12 @@ while True: up.identify() print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) - except: + except Exception: # most probably a timeout talking to the port, no bootloader, try to reboot the board print("attempting reboot on %s..." % port) up.send_reboot() # wait for the reboot, without we might run into Serial I/O Error 5 - time.sleep(1.5) + time.sleep(0.5) continue try: diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py index 7fefc5908..5b1e55e22 100644 --- a/Tools/sdlog2_dump.py +++ b/Tools/sdlog2_dump.py @@ -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 diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore new file mode 100644 index 000000000..61e091551 --- /dev/null +++ b/Tools/tests-host/.gitignore @@ -0,0 +1,2 @@ +./obj/* +mixer_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile new file mode 100644 index 000000000..97410ff47 --- /dev/null +++ b/Tools/tests-host/Makefile @@ -0,0 +1,39 @@ + +CC=g++ +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" + +ODIR=obj +LDIR =../lib + +LIBS=-lm + +#_DEPS = test.h +#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) + +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) + +#$(DEPS) +$(ODIR)/%.o: %.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c + $(CC) -c -o $@ $< $(CFLAGS) + +# +mixer_test: $(OBJ) + g++ -o $@ $^ $(CFLAGS) $(LIBS) + +.PHONY: clean + +clean: + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file diff --git a/Tools/tests-host/arch/board/board.h b/Tools/tests-host/arch/board/board.h new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/Tools/tests-host/arch/board/board.h diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp new file mode 100644 index 000000000..042322aad --- /dev/null +++ b/Tools/tests-host/mixer_test.cpp @@ -0,0 +1,12 @@ +#include <systemlib/mixer/mixer.h> +#include <systemlib/err.h> +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("Host execution started"); + + char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix", + "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; + + test_mixer(3, args); +}
\ No newline at end of file diff --git a/Tools/tests-host/nuttx/config.h b/Tools/tests-host/nuttx/config.h new file mode 100644 index 000000000..e69de29bb --- /dev/null +++ b/Tools/tests-host/nuttx/config.h diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3e94c3534..f9061c110 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -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 @@ -63,6 +63,7 @@ MODULES += systemcmds/nshterm # General system control # MODULES += modules/commander +MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard MODULES += modules/gpio_led @@ -78,9 +79,9 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control +MODULES += modules/segway +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control MODULES += examples/flow_position_control @@ -111,8 +112,10 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter -#MODULES += lib/ecl +MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps @@ -132,7 +135,10 @@ MODULES += lib/geo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bd73bde5e..ed2f2da5e 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -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 @@ -51,6 +52,7 @@ MODULES += systemcmds/param MODULES += systemcmds/perf MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests @@ -61,6 +63,7 @@ MODULES += systemcmds/nshterm # General system control # MODULES += modules/commander +MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard @@ -75,8 +78,9 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control +MODULES += modules/segway +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control @@ -105,8 +109,10 @@ MODULES += modules/uORB LIBRARIES += lib/mathlib/CMSIS MODULES += lib/mathlib MODULES += lib/mathlib/math/filter -#MODULES += lib/ecl +MODULES += lib/ecl +MODULES += lib/external_lgpl MODULES += lib/geo +MODULES += lib/conversion # # Demo apps @@ -124,6 +130,13 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index b3e50501c..cb20d9cd1 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -110,6 +110,8 @@ ifneq ($(words $(PX4_BASE)),1) $(error Cannot build when the PX4_BASE path contains one or more space characters.) endif +$(info % GIT_DESC = $(GIT_DESC)) + # # Set a default target so that included makefiles or errors here don't # cause confusion. @@ -177,6 +179,12 @@ GLOBAL_DEPS += $(MAKEFILE_LIST) # EXTRA_CLEANS = + +# +# Extra defines for compilation +# +export EXTRADEFINES := -DGIT_VERSION=$(GIT_DESC) + # # Append the per-board driver directory to the header search path. # diff --git a/makefiles/upload.mk b/makefiles/upload.mk index 470ddfdf1..bc26d743d 100644 --- a/makefiles/upload.mk +++ b/makefiles/upload.mk @@ -12,10 +12,10 @@ SYSTYPE := $(shell uname -s) # XXX The uploader should be smarter than this. # ifeq ($(SYSTYPE),Darwin) -SERIAL_PORTS ?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4,/dev/tty.usbmodem1,/dev/tty.usbmodem2,/dev/tty.usbmodem3,/dev/tty.usbmodem4" +SERIAL_PORTS ?= "/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" endif ifeq ($(SYSTYPE),Linux) -SERIAL_PORTS ?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0" +SERIAL_PORTS ?= "/dev/serial/by-id/usb-3D_Robotics*" endif ifeq ($(SERIAL_PORTS),) SERIAL_PORTS = "COM32,COM31,COM30,COM29,COM28,COM27,COM26,COM25,COM24,COM23,COM22,COM21,COM20,COM19,COM18,COM17,COM16,COM15,COM14,COM13,COM12,COM11,COM10,COM9,COM8,COM7,COM6,COM5,COM4,COM3,COM2,COM1,COM0" diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index eec9a89c5..3aad8e678 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 111, 21, 21, 144, 1, 234, 73, 181, 22, 83, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -74,6 +74,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ @@ -86,7 +87,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif @@ -179,6 +181,7 @@ enum LIMIT_MODULE #include "./mavlink_msg_data64.h" #include "./mavlink_msg_data96.h" #include "./mavlink_msg_rangefinder.h" +#include "./mavlink_msg_airspeed_autocal.h" #ifdef __cplusplus } diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h index d9d2ccb04..c3ead1140 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t componen * @brief Pack a ahrs message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param omegaIx X gyro drift estimate rad/s * @param omegaIy Y gyro drift estimate rad/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ahrs struct into a message + * @brief Encode a ahrs struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a ahrs struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** * @brief Send a ahrs message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h new file mode 100644 index 000000000..d046f2ad0 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -0,0 +1,419 @@ +// MESSAGE AIRSPEED_AUTOCAL PACKING + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL 174 + +typedef struct __mavlink_airspeed_autocal_t +{ + float vx; ///< GPS velocity north m/s + float vy; ///< GPS velocity east m/s + float vz; ///< GPS velocity down m/s + float diff_pressure; ///< Differential pressure pascals + float EAS2TAS; ///< Estimated to true airspeed ratio + float ratio; ///< Airspeed ratio + float state_x; ///< EKF state x + float state_y; ///< EKF state y + float state_z; ///< EKF state z + float Pax; ///< EKF Pax + float Pby; ///< EKF Pby + float Pcz; ///< EKF Pcz +} mavlink_airspeed_autocal_t; + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN 48 +#define MAVLINK_MSG_ID_174_LEN 48 + +#define MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC 167 +#define MAVLINK_MSG_ID_174_CRC 167 + + + +#define MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL { \ + "AIRSPEED_AUTOCAL", \ + 12, \ + { { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_airspeed_autocal_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_airspeed_autocal_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_airspeed_autocal_t, vz) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_airspeed_autocal_t, diff_pressure) }, \ + { "EAS2TAS", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_airspeed_autocal_t, EAS2TAS) }, \ + { "ratio", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_airspeed_autocal_t, ratio) }, \ + { "state_x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_airspeed_autocal_t, state_x) }, \ + { "state_y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_airspeed_autocal_t, state_y) }, \ + { "state_z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_airspeed_autocal_t, state_z) }, \ + { "Pax", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_airspeed_autocal_t, Pax) }, \ + { "Pby", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_airspeed_autocal_t, Pby) }, \ + { "Pcz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_airspeed_autocal_t, Pcz) }, \ + } \ +} + + +/** + * @brief Pack a airspeed_autocal message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Pack a airspeed_autocal message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_airspeed_autocal_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float vx,float vy,float vz,float diff_pressure,float EAS2TAS,float ratio,float state_x,float state_y,float state_z,float Pax,float Pby,float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIRSPEED_AUTOCAL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} + +/** + * @brief Encode a airspeed_autocal struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack(system_id, component_id, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Encode a airspeed_autocal struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeed_autocal C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeed_autocal_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeed_autocal_t* airspeed_autocal) +{ + return mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, chan, msg, airspeed_autocal->vx, airspeed_autocal->vy, airspeed_autocal->vz, airspeed_autocal->diff_pressure, airspeed_autocal->EAS2TAS, airspeed_autocal->ratio, airspeed_autocal->state_x, airspeed_autocal->state_y, airspeed_autocal->state_z, airspeed_autocal->Pax, airspeed_autocal->Pby, airspeed_autocal->Pcz); +} + +/** + * @brief Send a airspeed_autocal message + * @param chan MAVLink channel to send the message + * + * @param vx GPS velocity north m/s + * @param vy GPS velocity east m/s + * @param vz GPS velocity down m/s + * @param diff_pressure Differential pressure pascals + * @param EAS2TAS Estimated to true airspeed ratio + * @param ratio Airspeed ratio + * @param state_x EKF state x + * @param state_y EKF state y + * @param state_z EKF state z + * @param Pax EKF Pax + * @param Pby EKF Pby + * @param Pcz EKF Pcz + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN]; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t packet; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.diff_pressure = diff_pressure; + packet.EAS2TAS = EAS2TAS; + packet.ratio = ratio; + packet.state_x = state_x; + packet.state_y = state_y; + packet.state_z = state_z; + packet.Pax = Pax; + packet.Pby = Pby; + packet.Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)&packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} + +#endif + +// MESSAGE AIRSPEED_AUTOCAL UNPACKING + + +/** + * @brief Get field vx from airspeed_autocal message + * + * @return GPS velocity north m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field vy from airspeed_autocal message + * + * @return GPS velocity east m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field vz from airspeed_autocal message + * + * @return GPS velocity down m/s + */ +static inline float mavlink_msg_airspeed_autocal_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field diff_pressure from airspeed_autocal message + * + * @return Differential pressure pascals + */ +static inline float mavlink_msg_airspeed_autocal_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field EAS2TAS from airspeed_autocal message + * + * @return Estimated to true airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_EAS2TAS(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field ratio from airspeed_autocal message + * + * @return Airspeed ratio + */ +static inline float mavlink_msg_airspeed_autocal_get_ratio(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field state_x from airspeed_autocal message + * + * @return EKF state x + */ +static inline float mavlink_msg_airspeed_autocal_get_state_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field state_y from airspeed_autocal message + * + * @return EKF state y + */ +static inline float mavlink_msg_airspeed_autocal_get_state_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field state_z from airspeed_autocal message + * + * @return EKF state z + */ +static inline float mavlink_msg_airspeed_autocal_get_state_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field Pax from airspeed_autocal message + * + * @return EKF Pax + */ +static inline float mavlink_msg_airspeed_autocal_get_Pax(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field Pby from airspeed_autocal message + * + * @return EKF Pby + */ +static inline float mavlink_msg_airspeed_autocal_get_Pby(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field Pcz from airspeed_autocal message + * + * @return EKF Pcz + */ +static inline float mavlink_msg_airspeed_autocal_get_Pcz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a airspeed_autocal message into a struct + * + * @param msg The message to decode + * @param airspeed_autocal C-struct to decode the message contents into + */ +static inline void mavlink_msg_airspeed_autocal_decode(const mavlink_message_t* msg, mavlink_airspeed_autocal_t* airspeed_autocal) +{ +#if MAVLINK_NEED_BYTE_SWAP + airspeed_autocal->vx = mavlink_msg_airspeed_autocal_get_vx(msg); + airspeed_autocal->vy = mavlink_msg_airspeed_autocal_get_vy(msg); + airspeed_autocal->vz = mavlink_msg_airspeed_autocal_get_vz(msg); + airspeed_autocal->diff_pressure = mavlink_msg_airspeed_autocal_get_diff_pressure(msg); + airspeed_autocal->EAS2TAS = mavlink_msg_airspeed_autocal_get_EAS2TAS(msg); + airspeed_autocal->ratio = mavlink_msg_airspeed_autocal_get_ratio(msg); + airspeed_autocal->state_x = mavlink_msg_airspeed_autocal_get_state_x(msg); + airspeed_autocal->state_y = mavlink_msg_airspeed_autocal_get_state_y(msg); + airspeed_autocal->state_z = mavlink_msg_airspeed_autocal_get_state_z(msg); + airspeed_autocal->Pax = mavlink_msg_airspeed_autocal_get_Pax(msg); + airspeed_autocal->Pby = mavlink_msg_airspeed_autocal_get_Pby(msg); + airspeed_autocal->Pcz = mavlink_msg_airspeed_autocal_get_Pcz(msg); +#else + memcpy(airspeed_autocal, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h index c6c762451..821ce73e4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack(uint8_t system_id, uint8_t compon * @brief Pack a ap_adc message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC output 1 * @param adc2 ADC output 2 @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_ap_adc_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a ap_adc struct into a message + * @brief Encode a ap_adc struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_ap_adc_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a ap_adc struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ap_adc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ap_adc_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ap_adc_t* ap_adc) +{ + return mavlink_msg_ap_adc_pack_chan(system_id, component_id, chan, msg, ap_adc->adc1, ap_adc->adc2, ap_adc->adc3, ap_adc->adc4, ap_adc->adc5, ap_adc->adc6); +} + +/** * @brief Send a ap_adc message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h index c75358526..9200eefa0 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data16_pack(uint8_t system_id, uint8_t compon * @brief Pack a data16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data16_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data16 struct into a message + * @brief Encode a data16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data16_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data16_t* data16) +{ + return mavlink_msg_data16_pack_chan(system_id, component_id, chan, msg, data16->type, data16->len, data16->data); +} + +/** * @brief Send a data16 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h index 46c804a3c..3afedb787 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data32_pack(uint8_t system_id, uint8_t compon * @brief Pack a data32 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data32_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data32 struct into a message + * @brief Encode a data32 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data32_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data32 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data32 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data32_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data32_t* data32) +{ + return mavlink_msg_data32_pack_chan(system_id, component_id, chan, msg, data32->type, data32->len, data32->data); +} + +/** * @brief Send a data32 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h index 843084dff..6931ada16 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data64_pack(uint8_t system_id, uint8_t compon * @brief Pack a data64 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data64_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data64 struct into a message + * @brief Encode a data64 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data64_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data64 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data64 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data64_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data64_t* data64) +{ + return mavlink_msg_data64_pack_chan(system_id, component_id, chan, msg, data64->type, data64->len, data64->data); +} + +/** * @brief Send a data64 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h index 095628865..cffc7d7e7 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_data96_pack(uint8_t system_id, uint8_t compon * @brief Pack a data96 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type data type * @param len data length @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_data96_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a data96 struct into a message + * @brief Encode a data96 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_data96_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a data96 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data96 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data96_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data96_t* data96) +{ + return mavlink_msg_data96_pack_chan(system_id, component_id, chan, msg, data96->type, data96->len, data96->data); +} + +/** * @brief Send a data96 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h index bcc706a88..c6518c419 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack(uint8_t system_id, uin * @brief Pack a digicam_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_digicam_configure_pack_chan(uint8_t system_id } /** - * @brief Encode a digicam_configure struct into a message + * @brief Encode a digicam_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_digicam_configure_encode(uint8_t system_id, u } /** + * @brief Encode a digicam_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_configure_t* digicam_configure) +{ + return mavlink_msg_digicam_configure_pack_chan(system_id, component_id, chan, msg, digicam_configure->target_system, digicam_configure->target_component, digicam_configure->mode, digicam_configure->shutter_speed, digicam_configure->aperture, digicam_configure->iso, digicam_configure->exposure_type, digicam_configure->command_id, digicam_configure->engine_cut_off, digicam_configure->extra_param, digicam_configure->extra_value); +} + +/** * @brief Send a digicam_configure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h index 7fa8cdfef..bfa5414a3 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack(uint8_t system_id, uint8 * @brief Pack a digicam_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_digicam_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a digicam_control struct into a message + * @brief Encode a digicam_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_digicam_control_encode(uint8_t system_id, uin } /** + * @brief Encode a digicam_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param digicam_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_digicam_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_digicam_control_t* digicam_control) +{ + return mavlink_msg_digicam_control_pack_chan(system_id, component_id, chan, msg, digicam_control->target_system, digicam_control->target_component, digicam_control->session, digicam_control->zoom_pos, digicam_control->zoom_step, digicam_control->focus_lock, digicam_control->shot, digicam_control->command_id, digicam_control->extra_param, digicam_control->extra_value); +} + +/** * @brief Send a digicam_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h index 2cd4fc798..fe3677d53 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack(uint8_t system_id, uin * @brief Pack a fence_fetch_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_fence_fetch_point_pack_chan(uint8_t system_id } /** - * @brief Encode a fence_fetch_point struct into a message + * @brief Encode a fence_fetch_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_fence_fetch_point_encode(uint8_t system_id, u } /** + * @brief Encode a fence_fetch_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_fetch_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_fetch_point_t* fence_fetch_point) +{ + return mavlink_msg_fence_fetch_point_pack_chan(system_id, component_id, chan, msg, fence_fetch_point->target_system, fence_fetch_point->target_component, fence_fetch_point->idx); +} + +/** * @brief Send a fence_fetch_point message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h index b3c4706ee..febda6cdc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_fence_point_pack(uint8_t system_id, uint8_t c * @brief Pack a fence_point message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_fence_point_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a fence_point struct into a message + * @brief Encode a fence_point struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_fence_point_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a fence_point struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_point_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_point_t* fence_point) +{ + return mavlink_msg_fence_point_pack_chan(system_id, component_id, chan, msg, fence_point->target_system, fence_point->target_component, fence_point->idx, fence_point->count, fence_point->lat, fence_point->lng); +} + +/** * @brief Send a fence_point message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h index 32f2bc03a..612090406 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t * @brief Pack a fence_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param breach_status 0 if currently inside fence, 1 if outside * @param breach_count number of fence breaches @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a fence_status struct into a message + * @brief Encode a fence_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a fence_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param fence_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) +{ + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); +} + +/** * @brief Send a fence_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h index 73870ec0f..2f5dea513 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack(uint8_t system_id, uint8_t comp * @brief Pack a hwstatus message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Vcc board voltage (mV) * @param I2Cerr I2C error count @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_hwstatus_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hwstatus struct into a message + * @brief Encode a hwstatus struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_hwstatus_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a hwstatus struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hwstatus C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hwstatus_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hwstatus_t* hwstatus) +{ + return mavlink_msg_hwstatus_pack_chan(system_id, component_id, chan, msg, hwstatus->Vcc, hwstatus->I2Cerr); +} + +/** * @brief Send a hwstatus message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h index f7b04fba7..34743fd02 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t * @brief Pack a limits_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) * @param last_trigger time of last breach in milliseconds since boot @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a limits_status struct into a message + * @brief Encode a limits_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8 } /** + * @brief Encode a limits_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack_chan(system_id, component_id, chan, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** * @brief Send a limits_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h index 437029eed..55f772bbc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_meminfo_pack(uint8_t system_id, uint8_t compo * @brief Pack a meminfo message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param brkval heap top * @param freemem free memory @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_meminfo_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a meminfo struct into a message + * @brief Encode a meminfo struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_meminfo_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a meminfo struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param meminfo C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_meminfo_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_meminfo_t* meminfo) +{ + return mavlink_msg_meminfo_pack_chan(system_id, component_id, chan, msg, meminfo->brkval, meminfo->freemem); +} + +/** * @brief Send a meminfo message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h index 450153c6f..de717dfa4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack(uint8_t system_id, uint8 * @brief Pack a mount_configure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_configure_pack_chan(uint8_t system_id, } /** - * @brief Encode a mount_configure struct into a message + * @brief Encode a mount_configure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_configure_encode(uint8_t system_id, uin } /** + * @brief Encode a mount_configure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_configure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_configure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_configure_t* mount_configure) +{ + return mavlink_msg_mount_configure_pack_chan(system_id, component_id, chan, msg, mount_configure->target_system, mount_configure->target_component, mount_configure->mount_mode, mount_configure->stab_roll, mount_configure->stab_pitch, mount_configure->stab_yaw); +} + +/** * @brief Send a mount_configure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h index 5b83d7e97..44416353e 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_mount_control_pack(uint8_t system_id, uint8_t * @brief Pack a mount_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_mount_control_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mount_control struct into a message + * @brief Encode a mount_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_mount_control_encode(uint8_t system_id, uint8 } /** + * @brief Encode a mount_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_control_t* mount_control) +{ + return mavlink_msg_mount_control_pack_chan(system_id, component_id, chan, msg, mount_control->target_system, mount_control->target_component, mount_control->input_a, mount_control->input_b, mount_control->input_c, mount_control->save_position); +} + +/** * @brief Send a mount_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h index c031db42b..4905905dc 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_mount_status_pack(uint8_t system_id, uint8_t * @brief Pack a mount_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_mount_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mount_status struct into a message + * @brief Encode a mount_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_mount_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a mount_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_status_t* mount_status) +{ + return mavlink_msg_mount_status_pack_chan(system_id, component_id, chan, msg, mount_status->target_system, mount_status->target_component, mount_status->pointing_a, mount_status->pointing_b, mount_status->pointing_c); +} + +/** * @brief Send a mount_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h index e13776993..8e9740e82 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_pack(uint8_t system_id, uint8_t compone * @brief Pack a radio message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a radio struct into a message + * @brief Encode a radio struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_radio_encode(uint8_t system_id, uint8_t compo } /** + * @brief Encode a radio struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_t* radio) +{ + return mavlink_msg_radio_pack_chan(system_id, component_id, chan, msg, radio->rssi, radio->remrssi, radio->txbuf, radio->noise, radio->remnoise, radio->rxerrors, radio->fixed); +} + +/** * @brief Send a radio message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h index d88abe36a..c476447a8 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack(uint8_t system_id, uint8_t c * @brief Pack a rangefinder message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param distance distance in meters * @param voltage raw voltage if available, zero otherwise @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_rangefinder_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a rangefinder struct into a message + * @brief Encode a rangefinder struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_rangefinder_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a rangefinder struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rangefinder C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rangefinder_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rangefinder_t* rangefinder) +{ + return mavlink_msg_rangefinder_pack_chan(system_id, component_id, chan, msg, rangefinder->distance, rangefinder->voltage); +} + +/** * @brief Send a rangefinder message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h index d121e48e6..31b7d989d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_ * @brief Pack a sensor_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param mag_ofs_x magnetometer X offset * @param mag_ofs_y magnetometer Y offset @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u } /** - * @brief Encode a sensor_offsets struct into a message + * @brief Encode a sensor_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -190,6 +190,20 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint } /** + * @brief Encode a sensor_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sensor_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sensor_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets) +{ + return mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, chan, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z); +} + +/** * @brief Send a sensor_offsets message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h index a59e90649..b2c629c39 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8 * @brief Pack a set_mag_offsets message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_mag_offsets struct into a message + * @brief Encode a set_mag_offsets struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin } /** + * @brief Encode a set_mag_offsets struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mag_offsets C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mag_offsets_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets) +{ + return mavlink_msg_set_mag_offsets_pack_chan(system_id, component_id, chan, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z); +} + +/** * @brief Send a set_mag_offsets message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h index 7373c8bff..8ef44f76d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h @@ -13,15 +13,15 @@ typedef struct __mavlink_simstate_t float xgyro; ///< Angular speed around X axis rad/s float ygyro; ///< Angular speed around Y axis rad/s float zgyro; ///< Angular speed around Z axis rad/s - float lat; ///< Latitude in degrees - float lng; ///< Longitude in degrees + int32_t lat; ///< Latitude in degrees * 1E7 + int32_t lng; ///< Longitude in degrees * 1E7 } mavlink_simstate_t; #define MAVLINK_MSG_ID_SIMSTATE_LEN 44 #define MAVLINK_MSG_ID_164_LEN 44 -#define MAVLINK_MSG_ID_SIMSTATE_CRC 111 -#define MAVLINK_MSG_ID_164_CRC 111 +#define MAVLINK_MSG_ID_SIMSTATE_CRC 154 +#define MAVLINK_MSG_ID_164_CRC 154 @@ -37,8 +37,8 @@ typedef struct __mavlink_simstate_t { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_simstate_t, xgyro) }, \ { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_simstate_t, ygyro) }, \ { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_simstate_t, zgyro) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_simstate_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_simstate_t, lng) }, \ } \ } @@ -58,12 +58,12 @@ typedef struct __mavlink_simstate_t * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) + float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -76,8 +76,8 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @brief Pack a simstate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) @@ -120,13 +120,13 @@ static inline uint16_t mavlink_msg_simstate_pack(uint8_t system_id, uint8_t comp * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float lat,float lng) + float roll,float pitch,float yaw,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,int32_t lat,int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -139,8 +139,8 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SIMSTATE_LEN); #else @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_simstate_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a simstate struct into a message + * @brief Encode a simstate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a simstate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param simstate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_simstate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_simstate_t* simstate) +{ + return mavlink_msg_simstate_pack_chan(system_id, component_id, chan, msg, simstate->roll, simstate->pitch, simstate->yaw, simstate->xacc, simstate->yacc, simstate->zacc, simstate->xgyro, simstate->ygyro, simstate->zgyro, simstate->lat, simstate->lng); +} + +/** * @brief Send a simstate message * @param chan MAVLink channel to send the message * @@ -194,12 +208,12 @@ static inline uint16_t mavlink_msg_simstate_encode(uint8_t system_id, uint8_t co * @param xgyro Angular speed around X axis rad/s * @param ygyro Angular speed around Y axis rad/s * @param zgyro Angular speed around Z axis rad/s - * @param lat Latitude in degrees - * @param lng Longitude in degrees + * @param lat Latitude in degrees * 1E7 + * @param lng Longitude in degrees * 1E7 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lng) +static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SIMSTATE_LEN]; @@ -212,8 +226,8 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, _mav_put_float(buf, 24, xgyro); _mav_put_float(buf, 28, ygyro); _mav_put_float(buf, 32, zgyro); - _mav_put_float(buf, 36, lat); - _mav_put_float(buf, 40, lng); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); #if MAVLINK_CRC_EXTRA _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); @@ -340,21 +354,21 @@ static inline float mavlink_msg_simstate_get_zgyro(const mavlink_message_t* msg) /** * @brief Get field lat from simstate message * - * @return Latitude in degrees + * @return Latitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lat(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 36); + return _MAV_RETURN_int32_t(msg, 36); } /** * @brief Get field lng from simstate message * - * @return Longitude in degrees + * @return Longitude in degrees * 1E7 */ -static inline float mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) +static inline int32_t mavlink_msg_simstate_get_lng(const mavlink_message_t* msg) { - return _MAV_RETURN_float(msg, 40); + return _MAV_RETURN_int32_t(msg, 40); } /** diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h index ebdd2949d..7608a7bd1 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_wind_pack(uint8_t system_id, uint8_t componen * @brief Pack a wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param direction wind direction that wind is coming from (degrees) * @param speed wind speed in ground plane (m/s) @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_wind_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a wind struct into a message + * @brief Encode a wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_wind_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wind_t* wind) +{ + return mavlink_msg_wind_pack_chan(system_id, component_id, chan, msg, wind->direction, wind->speed, wind->speed_z); +} + +/** * @brief Send a wind message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index 07bf19ee0..2c9cfef3d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -738,8 +738,8 @@ static void mavlink_test_simstate(uint8_t system_id, uint8_t component_id, mavli 185.0, 213.0, 241.0, - 269.0, - 297.0, + 963499336, + 963499544, }; mavlink_simstate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1225,6 +1225,71 @@ static void mavlink_test_rangefinder(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_airspeed_autocal(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_airspeed_autocal_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + 269.0, + 297.0, + 325.0, + }; + mavlink_airspeed_autocal_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.diff_pressure = packet_in.diff_pressure; + packet1.EAS2TAS = packet_in.EAS2TAS; + packet1.ratio = packet_in.ratio; + packet1.state_x = packet_in.state_x; + packet1.state_y = packet_in.state_y; + packet1.state_z = packet_in.state_z; + packet1.Pax = packet_in.Pax; + packet1.Pby = packet_in.Pby; + packet1.Pcz = packet_in.Pcz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack(system_id, component_id, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_airspeed_autocal_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_airspeed_autocal_send(MAVLINK_COMM_1 , packet1.vx , packet1.vy , packet1.vz , packet1.diff_pressure , packet1.EAS2TAS , packet1.ratio , packet1.state_x , packet1.state_y , packet1.state_z , packet1.Pax , packet1.Pby , packet1.Pcz ); + mavlink_msg_airspeed_autocal_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_test_sensor_offsets(system_id, component_id, last_msg); @@ -1250,6 +1315,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_test_data64(system_id, component_id, last_msg); mavlink_test_data96(system_id, component_id, last_msg); mavlink_test_rangefinder(system_id, component_id, last_msg); + mavlink_test_airspeed_autocal(system_id, component_id, last_msg); } #ifdef __cplusplus diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h index 852b94b55..1b285ff7b 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:17 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:25 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index cc020b9d2..216c7805d 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -30,6 +30,30 @@ extern "C" { // ENUM DEFINITIONS +/** @brief Available operating modes/statuses for AutoQuad flight controller. + Bitmask up to 32 bits. Low side bits for base modes, high side for + additional active features/modifiers/constraints. */ +#ifndef HAVE_ENUM_AUTOQUAD_NAV_STATUS +#define HAVE_ENUM_AUTOQUAD_NAV_STATUS +enum AUTOQUAD_NAV_STATUS +{ + AQ_NAV_STATUS_INIT=0, /* System is initializing | */ + AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ + AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ + AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ + AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ + AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ + AQ_NAV_STATUS_CEILING=134217728, /* Ceiling altitude is set | */ + AQ_NAV_STATUS_HF_DYNAMIC=268435456, /* Heading-Free dynamic mode active | */ + AQ_NAV_STATUS_HF_LOCKED=536870912, /* Heading-Free locked mode active | */ + AQ_NAV_STATUS_RTH=1073741824, /* Automatic Return to Home is active | */ + AQ_NAV_STATUS_FAILSAFE=2147483648, /* System is in failsafe recovery mode | */ + AUTOQUAD_NAV_STATUS_ENUM_END=2147483649, /* | */ +}; +#endif + /** @brief */ #ifndef HAVE_ENUM_MAV_CMD #define HAVE_ENUM_MAV_CMD @@ -63,6 +87,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -71,7 +96,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h index dabccdf73..ed7c86bcb 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack(uint8_t system_id, uint8_ * @brief Pack a aq_telemetry_f message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param Index Index of message * @param value1 value1 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_pack_chan(uint8_t system_id, u } /** - * @brief Encode a aq_telemetry_f struct into a message + * @brief Encode a aq_telemetry_f struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -262,6 +262,20 @@ static inline uint16_t mavlink_msg_aq_telemetry_f_encode(uint8_t system_id, uint } /** + * @brief Encode a aq_telemetry_f struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param aq_telemetry_f C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_aq_telemetry_f_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_aq_telemetry_f_t* aq_telemetry_f) +{ + return mavlink_msg_aq_telemetry_f_pack_chan(system_id, component_id, chan, msg, aq_telemetry_f->Index, aq_telemetry_f->value1, aq_telemetry_f->value2, aq_telemetry_f->value3, aq_telemetry_f->value4, aq_telemetry_f->value5, aq_telemetry_f->value6, aq_telemetry_f->value7, aq_telemetry_f->value8, aq_telemetry_f->value9, aq_telemetry_f->value10, aq_telemetry_f->value11, aq_telemetry_f->value12, aq_telemetry_f->value13, aq_telemetry_f->value14, aq_telemetry_f->value15, aq_telemetry_f->value16, aq_telemetry_f->value17, aq_telemetry_f->value18, aq_telemetry_f->value19, aq_telemetry_f->value20); +} + +/** * @brief Send a aq_telemetry_f message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index a26ce2e90..d8ba74f03 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:37 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:36 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index e2ab66ed1..9d3db3326 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -260,6 +260,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -268,7 +269,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h deleted file mode 100644 index 0c4ab8c7d..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_6dof_setpoint.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE 6DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_6DOF_SETPOINT 149 - -typedef struct __mavlink_6dof_setpoint_t -{ - float trans_x; ///< Translational Component in x - float trans_y; ///< Translational Component in y - float trans_z; ///< Translational Component in z - float rot_x; ///< Rotational Component in x - float rot_y; ///< Rotational Component in y - float rot_z; ///< Rotational Component in z - uint8_t target_system; ///< System ID -} mavlink_6dof_setpoint_t; - -#define MAVLINK_MSG_ID_6DOF_SETPOINT_LEN 25 -#define MAVLINK_MSG_ID_149_LEN 25 - - - -#define MAVLINK_MESSAGE_INFO_6DOF_SETPOINT { \ - "6DOF_SETPOINT", \ - 7, \ - { { "trans_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_6dof_setpoint_t, trans_x) }, \ - { "trans_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_6dof_setpoint_t, trans_y) }, \ - { "trans_z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_6dof_setpoint_t, trans_z) }, \ - { "rot_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_6dof_setpoint_t, rot_x) }, \ - { "rot_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_6dof_setpoint_t, rot_y) }, \ - { "rot_z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_6dof_setpoint_t, rot_z) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_6dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 6dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 25, 144); -} - -/** - * @brief Pack a 6dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_6dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float trans_x,float trans_y,float trans_z,float rot_x,float rot_y,float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); -#endif - - msg->msgid = MAVLINK_MSG_ID_6DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 144); -} - -/** - * @brief Encode a 6dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 6dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_6dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_6dof_setpoint_t* 6dof_setpoint) -{ - return mavlink_msg_6dof_setpoint_pack(system_id, component_id, msg, 6dof_setpoint->target_system, 6dof_setpoint->trans_x, 6dof_setpoint->trans_y, 6dof_setpoint->trans_z, 6dof_setpoint->rot_x, 6dof_setpoint->rot_y, 6dof_setpoint->rot_z); -} - -/** - * @brief Send a 6dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param trans_x Translational Component in x - * @param trans_y Translational Component in y - * @param trans_z Translational Component in z - * @param rot_x Rotational Component in x - * @param rot_y Rotational Component in y - * @param rot_z Rotational Component in z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_6dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[25]; - _mav_put_float(buf, 0, trans_x); - _mav_put_float(buf, 4, trans_y); - _mav_put_float(buf, 8, trans_z); - _mav_put_float(buf, 12, rot_x); - _mav_put_float(buf, 16, rot_y); - _mav_put_float(buf, 20, rot_z); - _mav_put_uint8_t(buf, 24, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, buf, 25, 144); -#else - mavlink_6dof_setpoint_t packet; - packet.trans_x = trans_x; - packet.trans_y = trans_y; - packet.trans_z = trans_z; - packet.rot_x = rot_x; - packet.rot_y = rot_y; - packet.rot_z = rot_z; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_6DOF_SETPOINT, (const char *)&packet, 25, 144); -#endif -} - -#endif - -// MESSAGE 6DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 6dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_6dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 24); -} - -/** - * @brief Get field trans_x from 6dof_setpoint message - * - * @return Translational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field trans_y from 6dof_setpoint message - * - * @return Translational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field trans_z from 6dof_setpoint message - * - * @return Translational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_trans_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field rot_x from 6dof_setpoint message - * - * @return Rotational Component in x - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field rot_y from 6dof_setpoint message - * - * @return Rotational Component in y - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field rot_z from 6dof_setpoint message - * - * @return Rotational Component in z - */ -static inline float mavlink_msg_6dof_setpoint_get_rot_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Decode a 6dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 6dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_6dof_setpoint_decode(const mavlink_message_t* msg, mavlink_6dof_setpoint_t* 6dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 6dof_setpoint->trans_x = mavlink_msg_6dof_setpoint_get_trans_x(msg); - 6dof_setpoint->trans_y = mavlink_msg_6dof_setpoint_get_trans_y(msg); - 6dof_setpoint->trans_z = mavlink_msg_6dof_setpoint_get_trans_z(msg); - 6dof_setpoint->rot_x = mavlink_msg_6dof_setpoint_get_rot_x(msg); - 6dof_setpoint->rot_y = mavlink_msg_6dof_setpoint_get_rot_y(msg); - 6dof_setpoint->rot_z = mavlink_msg_6dof_setpoint_get_rot_z(msg); - 6dof_setpoint->target_system = mavlink_msg_6dof_setpoint_get_target_system(msg); -#else - memcpy(6dof_setpoint, _MAV_PAYLOAD(msg), 25); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h deleted file mode 100644 index 521d2fb66..000000000 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_8dof_setpoint.h +++ /dev/null @@ -1,320 +0,0 @@ -// MESSAGE 8DOF_SETPOINT PACKING - -#define MAVLINK_MSG_ID_8DOF_SETPOINT 148 - -typedef struct __mavlink_8dof_setpoint_t -{ - float val1; ///< Value 1 - float val2; ///< Value 2 - float val3; ///< Value 3 - float val4; ///< Value 4 - float val5; ///< Value 5 - float val6; ///< Value 6 - float val7; ///< Value 7 - float val8; ///< Value 8 - uint8_t target_system; ///< System ID -} mavlink_8dof_setpoint_t; - -#define MAVLINK_MSG_ID_8DOF_SETPOINT_LEN 33 -#define MAVLINK_MSG_ID_148_LEN 33 - - - -#define MAVLINK_MESSAGE_INFO_8DOF_SETPOINT { \ - "8DOF_SETPOINT", \ - 9, \ - { { "val1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_8dof_setpoint_t, val1) }, \ - { "val2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_8dof_setpoint_t, val2) }, \ - { "val3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_8dof_setpoint_t, val3) }, \ - { "val4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_8dof_setpoint_t, val4) }, \ - { "val5", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_8dof_setpoint_t, val5) }, \ - { "val6", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_8dof_setpoint_t, val6) }, \ - { "val7", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_8dof_setpoint_t, val7) }, \ - { "val8", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_8dof_setpoint_t, val8) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_8dof_setpoint_t, target_system) }, \ - } \ -} - - -/** - * @brief Pack a 8dof_setpoint message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 33, 42); -} - -/** - * @brief Pack a 8dof_setpoint message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_8dof_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,float val1,float val2,float val3,float val4,float val5,float val6,float val7,float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 33); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 33); -#endif - - msg->msgid = MAVLINK_MSG_ID_8DOF_SETPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 42); -} - -/** - * @brief Encode a 8dof_setpoint struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param 8dof_setpoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_8dof_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_8dof_setpoint_t* 8dof_setpoint) -{ - return mavlink_msg_8dof_setpoint_pack(system_id, component_id, msg, 8dof_setpoint->target_system, 8dof_setpoint->val1, 8dof_setpoint->val2, 8dof_setpoint->val3, 8dof_setpoint->val4, 8dof_setpoint->val5, 8dof_setpoint->val6, 8dof_setpoint->val7, 8dof_setpoint->val8); -} - -/** - * @brief Send a 8dof_setpoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param val1 Value 1 - * @param val2 Value 2 - * @param val3 Value 3 - * @param val4 Value 4 - * @param val5 Value 5 - * @param val6 Value 6 - * @param val7 Value 7 - * @param val8 Value 8 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_8dof_setpoint_send(mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[33]; - _mav_put_float(buf, 0, val1); - _mav_put_float(buf, 4, val2); - _mav_put_float(buf, 8, val3); - _mav_put_float(buf, 12, val4); - _mav_put_float(buf, 16, val5); - _mav_put_float(buf, 20, val6); - _mav_put_float(buf, 24, val7); - _mav_put_float(buf, 28, val8); - _mav_put_uint8_t(buf, 32, target_system); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, buf, 33, 42); -#else - mavlink_8dof_setpoint_t packet; - packet.val1 = val1; - packet.val2 = val2; - packet.val3 = val3; - packet.val4 = val4; - packet.val5 = val5; - packet.val6 = val6; - packet.val7 = val7; - packet.val8 = val8; - packet.target_system = target_system; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_8DOF_SETPOINT, (const char *)&packet, 33, 42); -#endif -} - -#endif - -// MESSAGE 8DOF_SETPOINT UNPACKING - - -/** - * @brief Get field target_system from 8dof_setpoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_8dof_setpoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field val1 from 8dof_setpoint message - * - * @return Value 1 - */ -static inline float mavlink_msg_8dof_setpoint_get_val1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field val2 from 8dof_setpoint message - * - * @return Value 2 - */ -static inline float mavlink_msg_8dof_setpoint_get_val2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field val3 from 8dof_setpoint message - * - * @return Value 3 - */ -static inline float mavlink_msg_8dof_setpoint_get_val3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field val4 from 8dof_setpoint message - * - * @return Value 4 - */ -static inline float mavlink_msg_8dof_setpoint_get_val4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field val5 from 8dof_setpoint message - * - * @return Value 5 - */ -static inline float mavlink_msg_8dof_setpoint_get_val5(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field val6 from 8dof_setpoint message - * - * @return Value 6 - */ -static inline float mavlink_msg_8dof_setpoint_get_val6(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field val7 from 8dof_setpoint message - * - * @return Value 7 - */ -static inline float mavlink_msg_8dof_setpoint_get_val7(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field val8 from 8dof_setpoint message - * - * @return Value 8 - */ -static inline float mavlink_msg_8dof_setpoint_get_val8(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a 8dof_setpoint message into a struct - * - * @param msg The message to decode - * @param 8dof_setpoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_8dof_setpoint_decode(const mavlink_message_t* msg, mavlink_8dof_setpoint_t* 8dof_setpoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - 8dof_setpoint->val1 = mavlink_msg_8dof_setpoint_get_val1(msg); - 8dof_setpoint->val2 = mavlink_msg_8dof_setpoint_get_val2(msg); - 8dof_setpoint->val3 = mavlink_msg_8dof_setpoint_get_val3(msg); - 8dof_setpoint->val4 = mavlink_msg_8dof_setpoint_get_val4(msg); - 8dof_setpoint->val5 = mavlink_msg_8dof_setpoint_get_val5(msg); - 8dof_setpoint->val6 = mavlink_msg_8dof_setpoint_get_val6(msg); - 8dof_setpoint->val7 = mavlink_msg_8dof_setpoint_get_val7(msg); - 8dof_setpoint->val8 = mavlink_msg_8dof_setpoint_get_val8(msg); - 8dof_setpoint->target_system = mavlink_msg_8dof_setpoint_get_target_system(msg); -#else - memcpy(8dof_setpoint, _MAV_PAYLOAD(msg), 33); -#endif -} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 0ba416ee1..8ddf5bf09 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp * @brief Pack a attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param roll Roll angle (rad, -pi..+pi) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a attitude struct into a message + * @brief Encode a attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack_chan(system_id, component_id, chan, msg, attitude->time_boot_ms, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + +/** * @brief Send a attitude message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 611803f2b..9f8d58759 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @brief Pack a attitude_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param q1 Quaternion component 1 @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ } /** - * @brief Encode a attitude_quaternion struct into a message + * @brief Encode a attitude_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, } /** + * @brief Encode a attitude_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) +{ + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); +} + +/** * @brief Send a attitude_quaternion message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 030b564c9..5703a5987 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp * @brief Pack a auth_key message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param key key * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a auth_key struct into a message + * @brief Encode a auth_key struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a auth_key struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack_chan(system_id, component_id, chan, msg, auth_key->key); +} + +/** * @brief Send a auth_key message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 83c815984..c945aebae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ * @brief Pack a battery_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accu_id Accupack ID * @param voltage_cell_1 Battery voltage of cell 1, in millivolts (1 = 1 millivolt) @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u } /** - * @brief Encode a battery_status struct into a message + * @brief Encode a battery_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint } /** + * @brief Encode a battery_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param battery_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) +{ + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->accu_id, battery_status->voltage_cell_1, battery_status->voltage_cell_2, battery_status->voltage_cell_3, battery_status->voltage_cell_4, battery_status->voltage_cell_5, battery_status->voltage_cell_6, battery_status->current_battery, battery_status->battery_remaining); +} + +/** * @brief Send a battery_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index c7195dfca..0b6de930d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i * @brief Pack a change_operator_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System the GCS requests control for * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys } /** - * @brief Encode a change_operator_control struct into a message + * @brief Encode a change_operator_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system } /** + * @brief Encode a change_operator_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack_chan(system_id, component_id, chan, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + +/** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index 5cf98e77f..c6f6a28e4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst * @brief Pack a change_operator_control_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gcs_system_id ID of the GCS this message * @param control_request 0: request control of this MAV, 1: Release control of this MAV @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t } /** - * @brief Encode a change_operator_control_ack struct into a message + * @brief Encode a change_operator_control_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy } /** + * @brief Encode a change_operator_control_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, chan, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + +/** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index 82c2835de..dca2fe681 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command Command ID, as defined by MAV_CMD enum. * @param result See MAV_RESULT enum @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a command_ack struct into a message + * @brief Encode a command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); +} + +/** * @brief Send a command_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 5c44c6284..8f705c0dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t * @brief Pack a command_long message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System which should execute the command * @param target_component Component which should execute the command, 0 for all components @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a command_long struct into a message + * @brief Encode a command_long struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a command_long struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param command_long C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_long_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_long_t* command_long) +{ + return mavlink_msg_command_long_pack_chan(system_id, component_id, chan, msg, command_long->target_system, command_long->target_component, command_long->command, command_long->confirmation, command_long->param1, command_long->param2, command_long->param3, command_long->param4, command_long->param5, command_long->param6, command_long->param7); +} + +/** * @brief Send a command_long message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index 782ea9f26..dc0768e12 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t c * @brief Pack a data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param stream_id The ID of the requested data stream * @param message_rate The requested interval between two messages of this type @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a data_stream struct into a message + * @brief Encode a data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_stream_t* data_stream) +{ + return mavlink_msg_data_stream_pack_chan(system_id, component_id, chan, msg, data_stream->stream_id, data_stream->message_rate, data_stream->on_off); +} + +/** * @brief Send a data_stream message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index a11161918..9a6ed87ee 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone * @brief Pack a debug message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param ind index of debug variable @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co } /** - * @brief Encode a debug struct into a message + * @brief Encode a debug struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo } /** + * @brief Encode a debug struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack_chan(system_id, component_id, chan, msg, debug->time_boot_ms, debug->ind, debug->value); +} + +/** * @brief Send a debug message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 5ee4e323a..6cfc75212 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @brief Pack a debug_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param name Name * @param time_usec Timestamp @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a debug_vect struct into a message + * @brief Encode a debug_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a debug_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack_chan(system_id, component_id, chan, msg, debug_vect->name, debug_vect->time_usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + +/** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h index c026419a2..4f31698d5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack(uint8_t system_id * @brief Pack a file_transfer_dir_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dir_path Directory path to list @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_pack_chan(uint8_t syst } /** - * @brief Encode a file_transfer_dir_list struct into a message + * @brief Encode a file_transfer_dir_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_file_transfer_dir_list_encode(uint8_t system_ } /** + * @brief Encode a file_transfer_dir_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_dir_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_dir_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_dir_list_t* file_transfer_dir_list) +{ + return mavlink_msg_file_transfer_dir_list_pack_chan(system_id, component_id, chan, msg, file_transfer_dir_list->transfer_uid, file_transfer_dir_list->dir_path, file_transfer_dir_list->flags); +} + +/** * @brief Send a file_transfer_dir_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h index 86d407d66..fc6247fac 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack(uint8_t system_id, uin * @brief Pack a file_transfer_res message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param result 0: OK, 1: not permitted, 2: bad path / file name, 3: no space left on device @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_file_transfer_res_pack_chan(uint8_t system_id } /** - * @brief Encode a file_transfer_res struct into a message + * @brief Encode a file_transfer_res struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_file_transfer_res_encode(uint8_t system_id, u } /** + * @brief Encode a file_transfer_res struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_res C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_res_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_res_t* file_transfer_res) +{ + return mavlink_msg_file_transfer_res_pack_chan(system_id, component_id, chan, msg, file_transfer_res->transfer_uid, file_transfer_res->result); +} + +/** * @brief Send a file_transfer_res message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h index 24bf25413..05be77339 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack(uint8_t system_id, u * @brief Pack a file_transfer_start message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param transfer_uid Unique transfer ID * @param dest_path Destination path @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_file_transfer_start_pack_chan(uint8_t system_ } /** - * @brief Encode a file_transfer_start struct into a message + * @brief Encode a file_transfer_start struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_file_transfer_start_encode(uint8_t system_id, } /** + * @brief Encode a file_transfer_start struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param file_transfer_start C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_file_transfer_start_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_file_transfer_start_t* file_transfer_start) +{ + return mavlink_msg_file_transfer_start_pack_chan(system_id, component_id, chan, msg, file_transfer_start->transfer_uid, file_transfer_start->dest_path, file_transfer_start->direction, file_transfer_start->file_size, file_transfer_start->flags); +} + +/** * @brief Send a file_transfer_start message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 11ab97ee4..7ed3d2a63 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -12,7 +12,7 @@ typedef struct __mavlink_global_position_int_t int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX } mavlink_global_position_int_t; #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 28 @@ -53,7 +53,7 @@ typedef struct __mavlink_global_position_int_t * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @brief Pack a global_position_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param lat Latitude, expressed as * 1E7 @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ } /** - * @brief Encode a global_position_int struct into a message + * @brief Encode a global_position_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, } /** + * @brief Encode a global_position_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack_chan(system_id, component_id, chan, msg, global_position_int->time_boot_ms, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->relative_alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); +} + +/** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message * @@ -177,7 +191,7 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -308,7 +322,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa /** * @brief Get field hdg from global_position_int message * - * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h index 8153628aa..1a1c97199 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t sys * @brief Pack a global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_ } /** - * @brief Encode a global_position_setpoint_int struct into a message + * @brief Encode a global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s } /** + * @brief Encode a global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_position_setpoint_int_t* global_position_setpoint_int) +{ + return mavlink_msg_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, global_position_setpoint_int->coordinate_frame, global_position_setpoint_int->latitude, global_position_setpoint_int->longitude, global_position_setpoint_int->altitude, global_position_setpoint_int->yaw); +} + +/** * @brief Send a global_position_setpoint_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index b388fa24a..f7be74c91 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t * @brief Pack a global_vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin } /** - * @brief Encode a global_vision_position_estimate struct into a message + * @brief Encode a global_vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_ } /** + * @brief Encode a global_vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param global_vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) +{ + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); +} + +/** * @brief Send a global_vision_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index bd09cb753..016e9cb0e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin * @brief Pack a gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param latitude Latitude (WGS84), in degrees * 1E7 * @param longitude Longitude (WGS84), in degrees * 1E7 @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id } /** - * @brief Encode a gps_global_origin struct into a message + * @brief Encode a gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u } /** + * @brief Encode a gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) +{ + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); +} + +/** * @brief Send a gps_global_origin message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index 2136b65ee..3054d4fda 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -8,10 +8,10 @@ typedef struct __mavlink_gps_raw_int_t int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 - uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -52,10 +52,10 @@ typedef struct __mavlink_gps_raw_int_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -104,17 +104,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @brief Pack a gps_raw_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @return length of the message in bytes (excluding serial stream start sign) */ @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a gps_raw_int struct into a message + * @brief Encode a gps_raw_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a gps_raw_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_raw_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); +} + +/** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message * @@ -182,10 +196,10 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param lat Latitude (WGS84), in degrees * 1E7 * @param lon Longitude (WGS84), in degrees * 1E7 * @param alt Altitude (WGS84), in meters * 1000 (positive for up) - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX + * @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -289,7 +303,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m /** * @brief Get field eph from gps_raw_int message * - * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* /** * @brief Get field epv from gps_raw_int message * - * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* /** * @brief Get field vel from gps_raw_int message * - * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* /** * @brief Get field cog from gps_raw_int message * - * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index cd6dde700..28d6b57d1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -86,7 +86,7 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @brief Pack a gps_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param satellites_visible Number of satellites visible * @param satellite_prn Global satellite ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a gps_status struct into a message + * @brief Encode a gps_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a gps_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack_chan(system_id, component_id, chan, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + +/** * @brief Send a gps_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index b2f0b65d0..826138fad 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @brief Pack a heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM @@ -127,7 +127,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a heartbeat struct into a message + * @brief Encode a heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -140,6 +140,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 6e7492ea6..0dcd95ed3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @brief Pack a highres_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a highres_imu struct into a message + * @brief Encode a highres_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -214,6 +214,20 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a highres_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param highres_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) +{ + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); +} + +/** * @brief Send a highres_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index 3319d3fd2..aed5108d0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t * @brief Pack a hil_controls message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll_ailerons Control output -1 .. 1 @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a hil_controls struct into a message + * @brief Encode a hil_controls struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a hil_controls struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack_chan(system_id, component_id, chan, msg, hil_controls->time_usec, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->aux1, hil_controls->aux2, hil_controls->aux3, hil_controls->aux4, hil_controls->mode, hil_controls->nav_mode); +} + +/** * @brief Send a hil_controls message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 75ab6835d..36a551872 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @brief Pack a hil_gps message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a hil_gps struct into a message + * @brief Encode a hil_gps struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -198,6 +198,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a hil_gps struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + +/** * @brief Send a hil_gps message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index 13e13d47c..acb1392e1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint * @brief Pack a hil_optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, } /** - * @brief Encode a hil_optical_flow struct into a message + * @brief Encode a hil_optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, ui } /** + * @brief Encode a hil_optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack_chan(system_id, component_id, chan, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); +} + +/** * @brief Send a hil_optical_flow message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index f2435dde8..a42bde50b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uin * @brief Pack a hil_rc_inputs_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param chan1_raw RC channel 1 value, in microseconds @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id } /** - * @brief Encode a hil_rc_inputs_raw struct into a message + * @brief Encode a hil_rc_inputs_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -206,6 +206,20 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u } /** + * @brief Encode a hil_rc_inputs_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_rc_inputs_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_rc_inputs_raw_t* hil_rc_inputs_raw) +{ + return mavlink_msg_hil_rc_inputs_raw_pack_chan(system_id, component_id, chan, msg, hil_rc_inputs_raw->time_usec, hil_rc_inputs_raw->chan1_raw, hil_rc_inputs_raw->chan2_raw, hil_rc_inputs_raw->chan3_raw, hil_rc_inputs_raw->chan4_raw, hil_rc_inputs_raw->chan5_raw, hil_rc_inputs_raw->chan6_raw, hil_rc_inputs_raw->chan7_raw, hil_rc_inputs_raw->chan8_raw, hil_rc_inputs_raw->chan9_raw, hil_rc_inputs_raw->chan10_raw, hil_rc_inputs_raw->chan11_raw, hil_rc_inputs_raw->chan12_raw, hil_rc_inputs_raw->rssi); +} + +/** * @brief Send a hil_rc_inputs_raw message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 422e55adc..6c2667473 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @brief Pack a hil_sensor message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param xacc X acceleration (m/s^2) @@ -201,7 +201,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a hil_sensor struct into a message + * @brief Encode a hil_sensor struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -214,6 +214,20 @@ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a hil_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + +/** * @brief Send a hil_sensor message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index 1d3f28664..bcc857767 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -134,7 +134,7 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com * @brief Pack a hil_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) @@ -209,7 +209,7 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a hil_state struct into a message + * @brief Encode a hil_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -222,6 +222,20 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a hil_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack_chan(system_id, component_id, chan, msg, hil_state->time_usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** * @brief Send a hil_state message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 0474e64a2..732176193 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -132,7 +132,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, * @brief Pack a hil_state_quaternion message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion @@ -205,7 +205,7 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system } /** - * @brief Encode a hil_state_quaternion struct into a message + * @brief Encode a hil_state_quaternion struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -218,6 +218,20 @@ static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id } /** + * @brief Encode a hil_state_quaternion struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, chan, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + +/** * @brief Send a hil_state_quaternion message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index 56723f3d7..a0b72c0e1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack(uint8_t system_id, ui * @brief Pack a local_position_ned message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_pack_chan(uint8_t system_i } /** - * @brief Encode a local_position_ned struct into a message + * @brief Encode a local_position_ned struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_encode(uint8_t system_id, } /** + * @brief Encode a local_position_ned struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_t* local_position_ned) +{ + return mavlink_msg_local_position_ned_pack_chan(system_id, component_id, chan, msg, local_position_ned->time_boot_ms, local_position_ned->x, local_position_ned->y, local_position_ned->z, local_position_ned->vx, local_position_ned->vy, local_position_ned->vz); +} + +/** * @brief Send a local_position_ned message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index c206a2906..8c4686202 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack( * @brief Pack a local_position_ned_system_global_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param x X Position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_pack_ } /** - * @brief Encode a local_position_ned_system_global_offset struct into a message + * @brief Encode a local_position_ned_system_global_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encod } /** + * @brief Encode a local_position_ned_system_global_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_ned_system_global_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_ned_system_global_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_ned_system_global_offset_t* local_position_ned_system_global_offset) +{ + return mavlink_msg_local_position_ned_system_global_offset_pack_chan(system_id, component_id, chan, msg, local_position_ned_system_global_offset->time_boot_ms, local_position_ned_system_global_offset->x, local_position_ned_system_global_offset->y, local_position_ned_system_global_offset->z, local_position_ned_system_global_offset->roll, local_position_ned_system_global_offset->pitch, local_position_ned_system_global_offset->yaw); +} + +/** * @brief Send a local_position_ned_system_global_offset message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h index 96f35fe62..1794815f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i * @brief Pack a local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys } /** - * @brief Encode a local_position_setpoint struct into a message + * @brief Encode a local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system } /** + * @brief Encode a local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) +{ + return mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, local_position_setpoint->coordinate_frame, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); +} + +/** * @brief Send a local_position_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index c9e4a4f8a..6b6e9e148 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @brief Pack a manual_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled. * @param x X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u } /** - * @brief Encode a manual_control struct into a message + * @brief Encode a manual_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint } /** + * @brief Encode a manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack_chan(system_id, component_id, chan, msg, manual_control->target, manual_control->x, manual_control->y, manual_control->z, manual_control->r, manual_control->buttons); +} + +/** * @brief Send a manual_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index d59e21292..a694947c1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack(uint8_t system_id, uint8 * @brief Pack a manual_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll rate in radians per second @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_manual_setpoint_pack_chan(uint8_t system_id, } /** - * @brief Encode a manual_setpoint struct into a message + * @brief Encode a manual_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_manual_setpoint_encode(uint8_t system_id, uin } /** + * @brief Encode a manual_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param manual_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_manual_setpoint_t* manual_setpoint) +{ + return mavlink_msg_manual_setpoint_pack_chan(system_id, component_id, chan, msg, manual_setpoint->time_boot_ms, manual_setpoint->roll, manual_setpoint->pitch, manual_setpoint->yaw, manual_setpoint->thrust, manual_setpoint->mode_switch, manual_setpoint->manual_override_switch); +} + +/** * @brief Send a manual_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index f8ae21b05..5f79329c2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c * @brief Pack a memory_vect message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param address Starting address of the debug variables * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a memory_vect struct into a message + * @brief Encode a memory_vect struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a memory_vect struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param memory_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_memory_vect_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_memory_vect_t* memory_vect) +{ + return mavlink_msg_memory_vect_pack_chan(system_id, component_id, chan, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); +} + +/** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 32825647f..7421d8394 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @brief Pack a mission_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a mission_ack struct into a message + * @brief Encode a mission_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a mission_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) +{ + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); +} + +/** * @brief Send a mission_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 06d2ac2e7..8f441c8e5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin * @brief Pack a mission_clear_all message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id } /** - * @brief Encode a mission_clear_all struct into a message + * @brief Encode a mission_clear_all struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u } /** + * @brief Encode a mission_clear_all struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_clear_all C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) +{ + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); +} + +/** * @brief Send a mission_clear_all message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index b28cec6f6..eac779306 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @brief Pack a mission_count message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a mission_count struct into a message + * @brief Encode a mission_count struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 } /** + * @brief Encode a mission_count struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_count C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) +{ + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); +} + +/** * @brief Send a mission_count message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index 5bf0899be..dbcdbd3f9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_current_pack(uint8_t system_id, uint8 * @brief Pack a mission_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_current_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_current struct into a message + * @brief Encode a mission_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_current_encode(uint8_t system_id, uin } /** + * @brief Encode a mission_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_current_t* mission_current) +{ + return mavlink_msg_mission_current_pack_chan(system_id, component_id, chan, msg, mission_current->seq); +} + +/** * @brief Send a mission_current message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index ed9d6e4af..634f80c37 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -124,7 +124,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @brief Pack a mission_item message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -193,7 +193,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a mission_item struct into a message + * @brief Encode a mission_item struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -206,6 +206,20 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a mission_item struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) +{ + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); +} + +/** * @brief Send a mission_item message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index 3f8a51a13..f3744fde6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack(uint8_t system_id, * @brief Pack a mission_item_reached message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seq Sequence * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_mission_item_reached_pack_chan(uint8_t system } /** - * @brief Encode a mission_item_reached struct into a message + * @brief Encode a mission_item_reached struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_mission_item_reached_encode(uint8_t system_id } /** + * @brief Encode a mission_item_reached struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_item_reached C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_item_reached_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_reached_t* mission_item_reached) +{ + return mavlink_msg_mission_item_reached_pack_chan(system_id, component_id, chan, msg, mission_item_reached->seq); +} + +/** * @brief Send a mission_item_reached message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index 0ced17614..ac84e3554 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 * @brief Pack a mission_request message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, } /** - * @brief Encode a mission_request struct into a message + * @brief Encode a mission_request struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin } /** + * @brief Encode a mission_request struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) +{ + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); +} + +/** * @brief Send a mission_request message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index 391df7f4d..d999babdb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, * @brief Pack a mission_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system } /** - * @brief Encode a mission_request_list struct into a message + * @brief Encode a mission_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id } /** + * @brief Encode a mission_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) +{ + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); +} + +/** * @brief Send a mission_request_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index d5a1c6939..35c7e1285 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys * @brief Pack a mission_request_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ } /** - * @brief Encode a mission_request_partial_list struct into a message + * @brief Encode a mission_request_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s } /** + * @brief Encode a mission_request_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_request_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) +{ + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); +} + +/** * @brief Send a mission_request_partial_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 2e145aa3e..63b37cf8c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack(uint8_t system_id, u * @brief Pack a mission_set_current message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_mission_set_current_pack_chan(uint8_t system_ } /** - * @brief Encode a mission_set_current struct into a message + * @brief Encode a mission_set_current struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_mission_set_current_encode(uint8_t system_id, } /** + * @brief Encode a mission_set_current struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_set_current C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_set_current_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_set_current_t* mission_set_current) +{ + return mavlink_msg_mission_set_current_pack_chan(system_id, component_id, chan, msg, mission_set_current->target_system, mission_set_current->target_component, mission_set_current->seq); +} + +/** * @brief Send a mission_set_current message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 6342f6038..7de38bd7b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste * @brief Pack a mission_write_partial_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t } /** - * @brief Encode a mission_write_partial_list struct into a message + * @brief Encode a mission_write_partial_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys } /** + * @brief Encode a mission_write_partial_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mission_write_partial_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) +{ + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); +} + +/** * @brief Send a mission_write_partial_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index a9d28a3d0..fbf4f75c9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin * @brief Pack a named_value_float message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id } /** - * @brief Encode a named_value_float struct into a message + * @brief Encode a named_value_float struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u } /** + * @brief Encode a named_value_float struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack_chan(system_id, component_id, chan, msg, named_value_float->time_boot_ms, named_value_float->name, named_value_float->value); +} + +/** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index ea53ea888..052f24793 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 * @brief Pack a named_value_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param name Name of the debug variable @@ -101,7 +101,7 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, } /** - * @brief Encode a named_value_int struct into a message + * @brief Encode a named_value_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -114,6 +114,20 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin } /** + * @brief Encode a named_value_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack_chan(system_id, component_id, chan, msg, named_value_int->time_boot_ms, named_value_int->name, named_value_int->value); +} + +/** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e9fa0f522..e95c144de 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, * @brief Pack a nav_controller_output message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param nav_roll Current desired roll in degrees * @param nav_pitch Current desired pitch in degrees @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste } /** - * @brief Encode a nav_controller_output struct into a message + * @brief Encode a nav_controller_output struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,6 +158,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i } /** + * @brief Encode a nav_controller_output struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, chan, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + +/** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h index c0e765a44..4debb6e66 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h @@ -83,7 +83,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack(uint8_t system_id, * @brief Pack a omnidirectional_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param sensor_id Sensor ID @@ -126,7 +126,7 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_pack_chan(uint8_t system } /** - * @brief Encode a omnidirectional_flow struct into a message + * @brief Encode a omnidirectional_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -139,6 +139,20 @@ static inline uint16_t mavlink_msg_omnidirectional_flow_encode(uint8_t system_id } /** + * @brief Encode a omnidirectional_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param omnidirectional_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_omnidirectional_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_omnidirectional_flow_t* omnidirectional_flow) +{ + return mavlink_msg_omnidirectional_flow_pack_chan(system_id, component_id, chan, msg, omnidirectional_flow->time_usec, omnidirectional_flow->sensor_id, omnidirectional_flow->left, omnidirectional_flow->right, omnidirectional_flow->quality, omnidirectional_flow->front_distance_m); +} + +/** * @brief Send a omnidirectional_flow message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index e01dc5e79..cf6db9147 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -8,8 +8,8 @@ typedef struct __mavlink_optical_flow_t float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance - int16_t flow_x; ///< Flow in pixels in x-sensor direction - int16_t flow_y; ///< Flow in pixels in y-sensor direction + int16_t flow_x; ///< Flow in pixels * 10 in x-sensor direction (dezi-pixels) + int16_t flow_y; ///< Flow in pixels * 10 in y-sensor direction (dezi-pixels) uint8_t sensor_id; ///< Sensor ID uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality } mavlink_optical_flow_t; @@ -45,8 +45,8 @@ typedef struct __mavlink_optical_flow_t * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -94,12 +94,12 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @brief Pack a optical_flow message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -145,7 +145,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a optical_flow struct into a message + * @brief Encode a optical_flow struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -158,13 +158,27 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a optical_flow struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** * @brief Send a optical_flow message * @param chan MAVLink channel to send the message * * @param time_usec Timestamp (UNIX) * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction + * @param flow_x Flow in pixels * 10 in x-sensor direction (dezi-pixels) + * @param flow_y Flow in pixels * 10 in y-sensor direction (dezi-pixels) * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality @@ -237,7 +251,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa /** * @brief Get field flow_x from optical_flow message * - * @return Flow in pixels in x-sensor direction + * @return Flow in pixels * 10 in x-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) { @@ -247,7 +261,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_ /** * @brief Get field flow_y from optical_flow message * - * @return Flow in pixels in y-sensor direction + * @return Flow in pixels * 10 in y-sensor direction (dezi-pixels) */ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index da61181b2..39e007274 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @brief Pack a param_request_list message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_list struct into a message + * @brief Encode a param_request_list struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, } /** + * @brief Encode a param_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack_chan(system_id, component_id, chan, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 6b1568026..5d9113114 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @brief Pack a param_request_read message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i } /** - * @brief Encode a param_request_read struct into a message + * @brief Encode a param_request_read struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, } /** + * @brief Encode a param_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack_chan(system_id, component_id, chan, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 66b0f6629..1bd1f0059 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @brief Pack a param_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a param_set struct into a message + * @brief Encode a param_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a param_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack_chan(system_id, component_id, chan, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value, param_set->param_type); +} + +/** * @brief Send a param_set message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 599139374..17c759811 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -77,7 +77,7 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @brief Pack a param_value message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string * @param param_value Onboard parameter value @@ -117,7 +117,7 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a param_value struct into a message + * @brief Encode a param_value struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -130,6 +130,20 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a param_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack_chan(system_id, component_id, chan, msg, param_value->param_id, param_value->param_value, param_value->param_type, param_value->param_count, param_value->param_index); +} + +/** * @brief Send a param_value message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 5a4c50907..0c68ca176 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @brief Pack a ping message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Unix timestamp in microseconds * @param seq PING sequence @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com } /** - * @brief Encode a ping struct into a message + * @brief Encode a ping struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon } /** + * @brief Encode a ping struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack_chan(system_id, component_id, chan, msg, ping->time_usec, ping->seq, ping->target_system, ping->target_component); +} + +/** * @brief Send a ping message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index 06e6a5542..fceb7d168 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t * @brief Pack a radio_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rssi local signal strength * @param remrssi remote signal strength @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a radio_status struct into a message + * @brief Encode a radio_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a radio_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack_chan(system_id, component_id, chan, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + +/** * @brief Send a radio_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index ce8863647..62a9b6eea 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param xacc X acceleration (raw) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_imu struct into a message + * @brief Encode a raw_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a raw_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + +/** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index dcc2cbe06..82c5fad4a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t * @brief Pack a raw_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param press_abs Absolute pressure (raw) @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a raw_pressure struct into a message + * @brief Encode a raw_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a raw_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack_chan(system_id, component_id, chan, msg, raw_pressure->time_usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + +/** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 9854a190c..0d8a1514b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -4,14 +4,14 @@ typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID } mavlink_rc_channels_override_t; @@ -49,14 +49,14 @@ typedef struct __mavlink_rc_channels_override_t * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -104,18 +104,18 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @brief Pack a rc_channels_override message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system } /** - * @brief Encode a rc_channels_override struct into a message + * @brief Encode a rc_channels_override struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,19 +174,33 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id } /** + * @brief Encode a rc_channels_override struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -259,7 +273,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons /** * @brief Get field chan1_raw from rc_channels_override message * - * @return RC channel 1 value, in microseconds + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { @@ -269,7 +283,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl /** * @brief Get field chan2_raw from rc_channels_override message * - * @return RC channel 2 value, in microseconds + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { @@ -279,7 +293,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl /** * @brief Get field chan3_raw from rc_channels_override message * - * @return RC channel 3 value, in microseconds + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { @@ -289,7 +303,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl /** * @brief Get field chan4_raw from rc_channels_override message * - * @return RC channel 4 value, in microseconds + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { @@ -299,7 +313,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl /** * @brief Get field chan5_raw from rc_channels_override message * - * @return RC channel 5 value, in microseconds + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { @@ -309,7 +323,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl /** * @brief Get field chan6_raw from rc_channels_override message * - * @return RC channel 6 value, in microseconds + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { @@ -319,7 +333,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl /** * @brief Get field chan7_raw from rc_channels_override message * - * @return RC channel 7 value, in microseconds + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { @@ -329,7 +343,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl /** * @brief Get field chan8_raw from rc_channels_override message * - * @return RC channel 8 value, in microseconds + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX means to ignore this field. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 4c1315bed..3c0aed020 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_raw_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_raw_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_raw_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @brief Pack a rc_channels_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a rc_channels_raw struct into a message + * @brief Encode a rc_channels_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin } /** + * @brief Encode a rc_channels_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, chan, msg, rc_channels_raw->time_boot_ms, rc_channels_raw->port, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + +/** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_raw RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan2_raw RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan3_raw RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan4_raw RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan5_raw RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan6_raw RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan7_raw RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. - * @param chan8_raw RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message /** * @brief Get field chan1_raw from rc_channels_raw message * - * @return RC channel 1 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m /** * @brief Get field chan2_raw from rc_channels_raw message * - * @return RC channel 2 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m /** * @brief Get field chan3_raw from rc_channels_raw message * - * @return RC channel 3 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m /** * @brief Get field chan4_raw from rc_channels_raw message * - * @return RC channel 4 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m /** * @brief Get field chan5_raw from rc_channels_raw message * - * @return RC channel 5 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m /** * @brief Get field chan6_raw from rc_channels_raw message * - * @return RC channel 6 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m /** * @brief Get field chan7_raw from rc_channels_raw message * - * @return RC channel 7 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m /** * @brief Get field chan8_raw from rc_channels_raw message * - * @return RC channel 8 value, in microseconds. A value of 65535 implies the channel is unused. + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index be6322bcd..2153ab392 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -5,14 +5,14 @@ typedef struct __mavlink_rc_channels_scaled_t { uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. uint8_t port; ///< Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. } mavlink_rc_channels_scaled_t; @@ -51,14 +51,14 @@ typedef struct __mavlink_rc_channels_scaled_t * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -109,18 +109,18 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @brief Pack a rc_channels_scaled message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i } /** - * @brief Encode a rc_channels_scaled struct into a message + * @brief Encode a rc_channels_scaled struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,19 +182,33 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, } /** + * @brief Encode a rc_channels_scaled struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, chan, msg, rc_channels_scaled->time_boot_ms, rc_channels_scaled->port, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + +/** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message * * @param time_boot_ms Timestamp (milliseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows for more than 8 servos. - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -270,7 +284,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_mess /** * @brief Get field chan1_scaled from rc_channels_scaled message * - * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { @@ -280,7 +294,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl /** * @brief Get field chan2_scaled from rc_channels_scaled message * - * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { @@ -290,7 +304,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl /** * @brief Get field chan3_scaled from rc_channels_scaled message * - * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { @@ -300,7 +314,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl /** * @brief Get field chan4_scaled from rc_channels_scaled message * - * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { @@ -310,7 +324,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl /** * @brief Get field chan5_scaled from rc_channels_scaled message * - * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { @@ -320,7 +334,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl /** * @brief Get field chan6_scaled from rc_channels_scaled message * - * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { @@ -330,7 +344,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl /** * @brief Get field chan7_scaled from rc_channels_scaled message * - * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { @@ -340,7 +354,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl /** * @brief Get field chan8_scaled from rc_channels_scaled message * - * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) 32767. + * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000, (invalid) INT16_MAX. */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index ee21d1fe0..c754ad876 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @brief Pack a request_data_stream message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ } /** - * @brief Encode a request_data_stream struct into a message + * @brief Encode a request_data_stream struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, } /** + * @brief Encode a request_data_stream struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack_chan(system_id, component_id, chan, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + +/** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h index a7e9df94b..ac3ef4fa9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_rates_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_rate Desired roll rate in radians per second @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode(u } /** + * @brief Encode a roll_pitch_yaw_rates_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_rates_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_rates_thrust_setpoint_t* roll_pitch_yaw_rates_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_rates_thrust_setpoint->time_boot_ms, roll_pitch_yaw_rates_thrust_setpoint->roll_rate, roll_pitch_yaw_rates_thrust_setpoint->pitch_rate, roll_pitch_yaw_rates_thrust_setpoint->yaw_rate, roll_pitch_yaw_rates_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_rates_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 517797655..626477322 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uin * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll_speed Desired roll angular speed in rad/s @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha } /** - * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u } /** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_speed_thrust_setpoint->time_boot_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index aee036022..ffcdc547b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t s * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp in milliseconds since system boot * @param roll Desired roll angle in radians @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint } /** - * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t } /** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, chan, msg, roll_pitch_yaw_thrust_setpoint->time_boot_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + +/** * @brief Send a roll_pitch_yaw_thrust_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index 100fabf16..fcd54cbb7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u * @brief Pack a safety_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. * @param p1x x position 1 / Latitude 1 @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ } /** - * @brief Encode a safety_allowed_area struct into a message + * @brief Encode a safety_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, } /** + * @brief Encode a safety_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index d365b7aed..61f6af155 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i * @brief Pack a safety_set_allowed_area message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys } /** - * @brief Encode a safety_set_allowed_area struct into a message + * @brief Encode a safety_set_allowed_area struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system } /** + * @brief Encode a safety_set_allowed_area struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, chan, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 2751ddfe7..3010d051a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @brief Pack a scaled_imu message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param xacc X acceleration (mg) @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a scaled_imu struct into a message + * @brief Encode a scaled_imu struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a scaled_imu struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + +/** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index f54e28195..10324bc94 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @brief Pack a scaled_pressure message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param press_abs Absolute pressure (hectopascal) @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, } /** - * @brief Encode a scaled_pressure struct into a message + * @brief Encode a scaled_pressure struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin } /** + * @brief Encode a scaled_pressure struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + +/** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 10bdcbc8c..6a14e93ed 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @brief Pack a servo_output_raw message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_usec Timestamp (microseconds since system boot) * @param port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, } /** - * @brief Encode a servo_output_raw struct into a message + * @brief Encode a servo_output_raw struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui } /** + * @brief Encode a servo_output_raw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h index 0364b4241..6f0d7a69d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t * @brief Pack a set_global_position_setpoint_int message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param coordinate_frame Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui } /** - * @brief Encode a set_global_position_setpoint_int struct into a message + * @brief Encode a set_global_position_setpoint_int struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8 } /** + * @brief Encode a set_global_position_setpoint_int struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_global_position_setpoint_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_global_position_setpoint_int_t* set_global_position_setpoint_int) +{ + return mavlink_msg_set_global_position_setpoint_int_pack_chan(system_id, component_id, chan, msg, set_global_position_setpoint_int->coordinate_frame, set_global_position_setpoint_int->latitude, set_global_position_setpoint_int->longitude, set_global_position_setpoint_int->altitude, set_global_position_setpoint_int->yaw); +} + +/** * @brief Send a set_global_position_setpoint_int message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index e3cd4f441..c444d8d52 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, * @brief Pack a set_gps_global_origin message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param latitude Latitude (WGS84), in degrees * 1E7 @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste } /** - * @brief Encode a set_gps_global_origin struct into a message + * @brief Encode a set_gps_global_origin struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i } /** + * @brief Encode a set_gps_global_origin struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_gps_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) +{ + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); +} + +/** * @brief Send a set_gps_global_origin message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h index b92c0560e..6f2835e03 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack(uint8_t syst * @brief Pack a set_local_position_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t } /** - * @brief Encode a set_local_position_setpoint struct into a message + * @brief Encode a set_local_position_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy } /** + * @brief Encode a set_local_position_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_local_position_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_local_position_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_local_position_setpoint_t* set_local_position_setpoint) +{ + return mavlink_msg_set_local_position_setpoint_pack_chan(system_id, component_id, chan, msg, set_local_position_setpoint->target_system, set_local_position_setpoint->target_component, set_local_position_setpoint->coordinate_frame, set_local_position_setpoint->x, set_local_position_setpoint->y, set_local_position_setpoint->z, set_local_position_setpoint->yaw); +} + +/** * @brief Send a set_local_position_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 08ec73309..1aff42cce 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp * @brief Pack a set_mode message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system The system setting the mode * @param base_mode The new base mode @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a set_mode struct into a message + * @brief Encode a set_mode struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a set_mode struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack_chan(system_id, component_id, chan, msg, set_mode->target_system, set_mode->base_mode, set_mode->custom_mode); +} + +/** * @brief Send a set_mode message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h index b79114e1a..8ceb8888f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack(uint8_t system_ * @brief Pack a set_quad_motors_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID of the system that should set these motor commands * @param motor_front_nw Front motor in + configuration, front left motor in x configuration @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_pack_chan(uint8_t sy } /** - * @brief Encode a set_quad_motors_setpoint struct into a message + * @brief Encode a set_quad_motors_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode(uint8_t syste } /** + * @brief Encode a set_quad_motors_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_motors_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_motors_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_motors_setpoint_t* set_quad_motors_setpoint) +{ + return mavlink_msg_set_quad_motors_setpoint_pack_chan(system_id, component_id, chan, msg, set_quad_motors_setpoint->target_system, set_quad_motors_setpoint->motor_front_nw, set_quad_motors_setpoint->motor_right_ne, set_quad_motors_setpoint->motor_back_se, set_quad_motors_setpoint->motor_left_sw); +} + +/** * @brief Send a set_quad_motors_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h index 06223845f..9ef294cc9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) uint8_t led_red[4]; ///< RGB red channel (0-255) @@ -56,10 +56,10 @@ typedef struct __mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -103,17 +103,17 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack * @brief Pack a set_quad_swarm_led_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -155,7 +155,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack } /** - * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -168,6 +168,20 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco } /** + * @brief Encode a set_quad_swarm_led_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_led_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t* set_quad_swarm_led_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_led_roll_pitch_yaw_thrust->group, set_quad_swarm_led_roll_pitch_yaw_thrust->mode, set_quad_swarm_led_roll_pitch_yaw_thrust->led_red, set_quad_swarm_led_roll_pitch_yaw_thrust->led_blue, set_quad_swarm_led_roll_pitch_yaw_thrust->led_green, set_quad_swarm_led_roll_pitch_yaw_thrust->roll, set_quad_swarm_led_roll_pitch_yaw_thrust->pitch, set_quad_swarm_led_roll_pitch_yaw_thrust->yaw, set_quad_swarm_led_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_quad_swarm_led_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * @@ -176,10 +190,10 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_enco * @param led_red RGB red channel (0-255) * @param led_blue RGB green channel (0-255) * @param led_green RGB blue channel (0-255) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -278,7 +292,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field roll from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -288,7 +302,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field pitch from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -298,7 +312,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field yaw from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -308,7 +322,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_ /** * @brief Get field thrust from set_quad_swarm_led_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h index 6c62b3530..7d8d526f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h @@ -4,10 +4,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t { - int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-32767) - int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-32767) - int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..65535) + int16_t roll[4]; ///< Desired roll angle in radians +-PI (+-INT16_MAX) + int16_t pitch[4]; ///< Desired pitch angle in radians +-PI (+-INT16_MAX) + int16_t yaw[4]; ///< Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + uint16_t thrust[4]; ///< Collective thrust, scaled to uint16 (0..UINT16_MAX) uint8_t group; ///< ID of the quadrotor group (0 - 255, up to 256 groups supported) uint8_t mode; ///< ID of the flight mode (0 - 255, up to 256 modes supported) } mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t; @@ -44,10 +44,10 @@ typedef struct __mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, @@ -85,14 +85,14 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack(uin * @brief Pack a set_quad_swarm_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, @@ -128,7 +128,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_cha } /** - * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -141,15 +141,29 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode(u } /** + * @brief Encode a set_quad_swarm_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_quad_swarm_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t* set_quad_swarm_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_quad_swarm_roll_pitch_yaw_thrust->group, set_quad_swarm_roll_pitch_yaw_thrust->mode, set_quad_swarm_roll_pitch_yaw_thrust->roll, set_quad_swarm_roll_pitch_yaw_thrust->pitch, set_quad_swarm_roll_pitch_yaw_thrust->yaw, set_quad_swarm_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_quad_swarm_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * * @param group ID of the quadrotor group (0 - 255, up to 256 groups supported) * @param mode ID of the flight mode (0 - 255, up to 256 modes supported) - * @param roll Desired roll angle in radians +-PI (+-32767) - * @param pitch Desired pitch angle in radians +-PI (+-32767) - * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-32767) - * @param thrust Collective thrust, scaled to uint16 (0..65535) + * @param roll Desired roll angle in radians +-PI (+-INT16_MAX) + * @param pitch Desired pitch angle in radians +-PI (+-INT16_MAX) + * @param yaw Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) + * @param thrust Collective thrust, scaled to uint16 (0..UINT16_MAX) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -212,7 +226,7 @@ static inline uint8_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_mode( /** * @brief Get field roll from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired roll angle in radians +-PI (+-32767) + * @return Desired roll angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg, int16_t *roll) { @@ -222,7 +236,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_roll /** * @brief Get field pitch from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired pitch angle in radians +-PI (+-32767) + * @return Desired pitch angle in radians +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg, int16_t *pitch) { @@ -232,7 +246,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_pitc /** * @brief Get field yaw from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Desired yaw angle in radians, scaled to int16 +-PI (+-32767) + * @return Desired yaw angle in radians, scaled to int16 +-PI (+-INT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg, int16_t *yaw) { @@ -242,7 +256,7 @@ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_yaw( /** * @brief Get field thrust from set_quad_swarm_roll_pitch_yaw_thrust message * - * @return Collective thrust, scaled to uint16 (0..65535) + * @return Collective thrust, scaled to uint16 (0..UINT16_MAX) */ static inline uint16_t mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg, uint16_t *thrust) { diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index c379a75d9..5846ba41f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin } /** - * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ } /** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + +/** * @brief Send a set_roll_pitch_yaw_speed_thrust message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 146891eaf..334fd39e3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system * @brief Pack a set_roll_pitch_yaw_thrust message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s } /** - * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * @brief Encode a set_roll_pitch_yaw_thrust struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst } /** + * @brief Encode a set_roll_pitch_yaw_thrust struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, chan, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + +/** * @brief Send a set_roll_pitch_yaw_thrust message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h index f352617a2..93ce345b6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_6dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param trans_x Translational Component in x @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_setpoint_6dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_6dof struct into a message + * @brief Encode a setpoint_6dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_setpoint_6dof_encode(uint8_t system_id, uint8 } /** + * @brief Encode a setpoint_6dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_6dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_6dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_6dof_t* setpoint_6dof) +{ + return mavlink_msg_setpoint_6dof_pack_chan(system_id, component_id, chan, msg, setpoint_6dof->target_system, setpoint_6dof->trans_x, setpoint_6dof->trans_y, setpoint_6dof->trans_z, setpoint_6dof->rot_x, setpoint_6dof->rot_y, setpoint_6dof->rot_z); +} + +/** * @brief Send a setpoint_6dof message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h index d7622b696..de80e4645 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack(uint8_t system_id, uint8_t * @brief Pack a setpoint_8dof message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param val1 Value 1 @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_setpoint_8dof_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a setpoint_8dof struct into a message + * @brief Encode a setpoint_8dof struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_setpoint_8dof_encode(uint8_t system_id, uint8 } /** + * @brief Encode a setpoint_8dof struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setpoint_8dof C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setpoint_8dof_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setpoint_8dof_t* setpoint_8dof) +{ + return mavlink_msg_setpoint_8dof_pack_chan(system_id, component_id, chan, msg, setpoint_8dof->target_system, setpoint_8dof->val1, setpoint_8dof->val2, setpoint_8dof->val3, setpoint_8dof->val4, setpoint_8dof->val5, setpoint_8dof->val6, setpoint_8dof->val7, setpoint_8dof->val8); +} + +/** * @brief Send a setpoint_8dof message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index 6fd32abd2..ebd657cf3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -159,7 +159,7 @@ static inline uint16_t mavlink_msg_sim_state_pack(uint8_t system_id, uint8_t com * @brief Pack a sim_state message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param q1 True attitude quaternion component 1 * @param q2 True attitude quaternion component 2 @@ -249,7 +249,7 @@ static inline uint16_t mavlink_msg_sim_state_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a sim_state struct into a message + * @brief Encode a sim_state struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -262,6 +262,20 @@ static inline uint16_t mavlink_msg_sim_state_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a sim_state struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sim_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sim_state_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sim_state_t* sim_state) +{ + return mavlink_msg_sim_state_pack_chan(system_id, component_id, chan, msg, sim_state->q1, sim_state->q2, sim_state->q3, sim_state->q4, sim_state->roll, sim_state->pitch, sim_state->yaw, sim_state->xacc, sim_state->yacc, sim_state->zacc, sim_state->xgyro, sim_state->ygyro, sim_state->zgyro, sim_state->lat, sim_state->lon, sim_state->alt, sim_state->std_dev_horz, sim_state->std_dev_vert, sim_state->vn, sim_state->ve, sim_state->vd); +} + +/** * @brief Send a sim_state message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h index 8a002fc11..2db627b96 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint * @brief Pack a state_correction message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param xErr x position error * @param yErr y position error @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, } /** - * @brief Encode a state_correction struct into a message + * @brief Encode a state_correction struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui } /** + * @brief Encode a state_correction struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param state_correction C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_state_correction_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) +{ + return mavlink_msg_state_correction_pack_chan(system_id, component_id, chan, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); +} + +/** * @brief Send a state_correction message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 103486863..536ca0634 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @brief Pack a statustext message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param severity Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. * @param text Status text message, without null termination character @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a statustext struct into a message + * @brief Encode a statustext struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -106,6 +106,20 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a statustext struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); +} + +/** * @brief Send a statustext message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 916bc4f07..058c630dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @brief Pack a sys_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control @@ -185,7 +185,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 } /** - * @brief Encode a sys_status struct into a message + * @brief Encode a sys_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -198,6 +198,20 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a sys_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack_chan(system_id, component_id, chan, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); +} + +/** * @brief Send a sys_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index b235fe205..1807567ae 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c * @brief Pack a system_time message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_unix_usec Timestamp of the master clock in microseconds since UNIX epoch. * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint } /** - * @brief Encode a system_time struct into a message + * @brief Encode a system_time struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t } /** + * @brief Encode a system_time struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param system_time C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack_chan(system_id, component_id, chan, msg, system_time->time_unix_usec, system_time->time_boot_ms); +} + +/** * @brief Send a system_time message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 9d459921f..5b1093a3d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo * @brief Pack a vfr_hud message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airspeed Current airspeed in m/s * @param groundspeed Current ground speed in m/s @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a vfr_hud struct into a message + * @brief Encode a vfr_hud struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a vfr_hud struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack_chan(system_id, component_id, chan, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + +/** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index 75e4b5b7a..a254202e4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @brief Pack a vicon_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys } /** - * @brief Encode a vicon_position_estimate struct into a message + * @brief Encode a vicon_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system } /** + * @brief Encode a vicon_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vicon_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) +{ + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); +} + +/** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index 47ccb11ec..f7a741b09 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @brief Pack a vision_position_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy } /** - * @brief Encode a vision_position_estimate struct into a message + * @brief Encode a vision_position_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste } /** + * @brief Encode a vision_position_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_position_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) +{ + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); +} + +/** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index c38eee62c..660225128 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @brief Pack a vision_speed_estimate message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds, synced to UNIX time or since system boot) * @param x Global X speed @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste } /** - * @brief Encode a vision_speed_estimate struct into a message + * @brief Encode a vision_speed_estimate struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i } /** + * @brief Encode a vision_speed_estimate struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param vision_speed_estimate C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) +{ + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); +} + +/** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/common/version.h b/mavlink/include/mavlink/v1.0/common/version.h index c91a641de..3c354f422 100644 --- a/mavlink/include/mavlink/v1.0/common/version.h +++ b/mavlink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:47:40 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:05 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h index f5576255e..f590cda80 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h @@ -77,6 +77,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -85,7 +86,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ }; #endif diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h index bd0be342f..bc47a547a 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack(uint8_t system_id, uint8_t com * @brief Pack a airspeeds message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param airspeed_imu Airspeed estimate from IMU, cm/s @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_airspeeds_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a airspeeds struct into a message + * @brief Encode a airspeeds struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_airspeeds_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a airspeeds struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param airspeeds C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_airspeeds_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_airspeeds_t* airspeeds) +{ + return mavlink_msg_airspeeds_pack_chan(system_id, component_id, chan, msg, airspeeds->time_boot_ms, airspeeds->airspeed_imu, airspeeds->airspeed_pitot, airspeeds->airspeed_hot_wire, airspeeds->airspeed_ultrasonic, airspeeds->aoa, airspeeds->aoy); +} + +/** * @brief Send a airspeeds message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h index c1ce66875..e64787cc7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_altitudes_pack(uint8_t system_id, uint8_t com * @brief Pack a altitudes message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms Timestamp (milliseconds since system boot) * @param alt_gps GPS altitude in meters, expressed as * 1000 (millimeters), above MSL @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_altitudes_pack_chan(uint8_t system_id, uint8_ } /** - * @brief Encode a altitudes struct into a message + * @brief Encode a altitudes struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_altitudes_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a altitudes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param altitudes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_altitudes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_altitudes_t* altitudes) +{ + return mavlink_msg_altitudes_pack_chan(system_id, component_id, chan, msg, altitudes->time_boot_ms, altitudes->alt_gps, altitudes->alt_imu, altitudes->alt_barometric, altitudes->alt_optical_flow, altitudes->alt_range_finder, altitudes->alt_extra); +} + +/** * @brief Send a altitudes message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h index d72a4dcac..581bd35dc 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h @@ -87,7 +87,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack(uint8_t sy * @brief Pack a flexifunction_buffer_function message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -133,7 +133,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_pack_chan(uint8 } /** - * @brief Encode a flexifunction_buffer_function struct into a message + * @brief Encode a flexifunction_buffer_function struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -146,6 +146,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode(uint8_t } /** + * @brief Encode a flexifunction_buffer_function struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_t* flexifunction_buffer_function) +{ + return mavlink_msg_flexifunction_buffer_function_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function->target_system, flexifunction_buffer_function->target_component, flexifunction_buffer_function->func_index, flexifunction_buffer_function->func_count, flexifunction_buffer_function->data_address, flexifunction_buffer_function->data_size, flexifunction_buffer_function->data); +} + +/** * @brief Send a flexifunction_buffer_function message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h index 58f1786ef..790afd52b 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack(uint8_ * @brief Pack a flexifunction_buffer_function_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_pack_chan(u } /** - * @brief Encode a flexifunction_buffer_function_ack struct into a message + * @brief Encode a flexifunction_buffer_function_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode(uint } /** + * @brief Encode a flexifunction_buffer_function_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_buffer_function_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_buffer_function_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_buffer_function_ack_t* flexifunction_buffer_function_ack) +{ + return mavlink_msg_flexifunction_buffer_function_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_buffer_function_ack->target_system, flexifunction_buffer_function_ack->target_component, flexifunction_buffer_function_ack->func_index, flexifunction_buffer_function_ack->result); +} + +/** * @brief Send a flexifunction_buffer_function_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h index 2f6668cf9..ce722c8a4 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h @@ -69,7 +69,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack(uint8_t system_id, * @brief Pack a flexifunction_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -105,7 +105,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_pack_chan(uint8_t syste } /** - * @brief Encode a flexifunction_command struct into a message + * @brief Encode a flexifunction_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -118,6 +118,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_encode(uint8_t system_i } /** + * @brief Encode a flexifunction_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_t* flexifunction_command) +{ + return mavlink_msg_flexifunction_command_pack_chan(system_id, component_id, chan, msg, flexifunction_command->target_system, flexifunction_command->target_component, flexifunction_command->command_type); +} + +/** * @brief Send a flexifunction_command message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h index 4798febb0..070dc4bf8 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack(uint8_t system * @brief Pack a flexifunction_command_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param command_type Command acknowledged * @param result result of acknowledge @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_pack_chan(uint8_t s } /** - * @brief Encode a flexifunction_command_ack struct into a message + * @brief Encode a flexifunction_command_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_command_ack_encode(uint8_t syst } /** + * @brief Encode a flexifunction_command_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_command_ack_t* flexifunction_command_ack) +{ + return mavlink_msg_flexifunction_command_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_command_ack->command_type, flexifunction_command_ack->result); +} + +/** * @brief Send a flexifunction_command_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h index 947bfc591..ef262a6b1 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h @@ -82,7 +82,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack(uint8_t system_i * @brief Pack a flexifunction_directory message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -125,7 +125,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_pack_chan(uint8_t sys } /** - * @brief Encode a flexifunction_directory struct into a message + * @brief Encode a flexifunction_directory struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -138,6 +138,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_encode(uint8_t system } /** + * @brief Encode a flexifunction_directory struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_t* flexifunction_directory) +{ + return mavlink_msg_flexifunction_directory_pack_chan(system_id, component_id, chan, msg, flexifunction_directory->target_system, flexifunction_directory->target_component, flexifunction_directory->directory_type, flexifunction_directory->start_index, flexifunction_directory->count, flexifunction_directory->directory_data); +} + +/** * @brief Send a flexifunction_directory message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h index 5489dd6b5..d3a386ce5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack(uint8_t syst * @brief Pack a flexifunction_directory_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_pack_chan(uint8_t } /** - * @brief Encode a flexifunction_directory_ack struct into a message + * @brief Encode a flexifunction_directory_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode(uint8_t sy } /** + * @brief Encode a flexifunction_directory_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_directory_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_directory_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_directory_ack_t* flexifunction_directory_ack) +{ + return mavlink_msg_flexifunction_directory_ack_pack_chan(system_id, component_id, chan, msg, flexifunction_directory_ack->target_system, flexifunction_directory_ack->target_component, flexifunction_directory_ack->directory_type, flexifunction_directory_ack->start_index, flexifunction_directory_ack->count, flexifunction_directory_ack->result); +} + +/** * @brief Send a flexifunction_directory_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h index 9ffc2caa5..e50f77b08 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack(uint8_t system_id * @brief Pack a flexifunction_read_req message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_pack_chan(uint8_t syst } /** - * @brief Encode a flexifunction_read_req struct into a message + * @brief Encode a flexifunction_read_req struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_flexifunction_read_req_encode(uint8_t system_ } /** + * @brief Encode a flexifunction_read_req struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_read_req C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_read_req_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_read_req_t* flexifunction_read_req) +{ + return mavlink_msg_flexifunction_read_req_pack_chan(system_id, component_id, chan, msg, flexifunction_read_req->target_system, flexifunction_read_req->target_component, flexifunction_read_req->read_req_type, flexifunction_read_req->data_index); +} + +/** * @brief Send a flexifunction_read_req message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h index fc7d1d2f8..41afb3811 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack(uint8_t system_id, uin * @brief Pack a flexifunction_set message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_flexifunction_set_pack_chan(uint8_t system_id } /** - * @brief Encode a flexifunction_set struct into a message + * @brief Encode a flexifunction_set struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_flexifunction_set_encode(uint8_t system_id, u } /** + * @brief Encode a flexifunction_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flexifunction_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flexifunction_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flexifunction_set_t* flexifunction_set) +{ + return mavlink_msg_flexifunction_set_pack_chan(system_id, component_id, chan, msg, flexifunction_set->target_system, flexifunction_set->target_component); +} + +/** * @brief Send a flexifunction_set message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h index df5645e76..d21ab4816 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f13 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_week_no Serial UDB Extra GPS Week Number * @param sue_lat_origin Serial UDB Extra MP Origin Latitude @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f13 struct into a message + * @brief Encode a serial_udb_extra_f13 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f13 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f13 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f13_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f13_t* serial_udb_extra_f13) +{ + return mavlink_msg_serial_udb_extra_f13_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f13->sue_week_no, serial_udb_extra_f13->sue_lat_origin, serial_udb_extra_f13->sue_lon_origin, serial_udb_extra_f13->sue_alt_origin); +} + +/** * @brief Send a serial_udb_extra_f13 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h index 5e38a590a..6ac12f28a 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f14 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_WIND_ESTIMATION Serial UDB Extra Wind Estimation Enabled * @param sue_GPS_TYPE Serial UDB Extra Type of GPS Unit @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f14 struct into a message + * @brief Encode a serial_udb_extra_f14 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -182,6 +182,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f14 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f14 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f14_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f14_t* serial_udb_extra_f14) +{ + return mavlink_msg_serial_udb_extra_f14_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f14->sue_WIND_ESTIMATION, serial_udb_extra_f14->sue_GPS_TYPE, serial_udb_extra_f14->sue_DR, serial_udb_extra_f14->sue_BOARD_TYPE, serial_udb_extra_f14->sue_AIRFRAME, serial_udb_extra_f14->sue_RCON, serial_udb_extra_f14->sue_TRAP_FLAGS, serial_udb_extra_f14->sue_TRAP_SOURCE, serial_udb_extra_f14->sue_osc_fail_count, serial_udb_extra_f14->sue_CLOCK_CONFIG, serial_udb_extra_f14->sue_FLIGHT_PLAN_TYPE); +} + +/** * @brief Send a serial_udb_extra_f14 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h index 8779b25ff..10c3f4ca4 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f15 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_VEHICLE_MODEL_NAME Serial UDB Extra Model Name Of Vehicle * @param sue_ID_VEHICLE_REGISTRATION Serial UDB Extra Registraton Number of Vehicle @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f15 struct into a message + * @brief Encode a serial_udb_extra_f15 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -111,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f15 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f15 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f15_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f15_t* serial_udb_extra_f15) +{ + return mavlink_msg_serial_udb_extra_f15_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f15->sue_ID_VEHICLE_MODEL_NAME, serial_udb_extra_f15->sue_ID_VEHICLE_REGISTRATION); +} + +/** * @brief Send a serial_udb_extra_f15 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h index 1a173bfe4..659e6b7c5 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h @@ -65,7 +65,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f16 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ID_LEAD_PILOT Serial UDB Extra Name of Expected Lead Pilot * @param sue_ID_DIY_DRONES_URL Serial UDB Extra URL of Lead Pilot or Team @@ -98,7 +98,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_pack_chan(uint8_t system } /** - * @brief Encode a serial_udb_extra_f16 struct into a message + * @brief Encode a serial_udb_extra_f16 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -111,6 +111,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode(uint8_t system_id } /** + * @brief Encode a serial_udb_extra_f16 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f16 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f16_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f16_t* serial_udb_extra_f16) +{ + return mavlink_msg_serial_udb_extra_f16_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f16->sue_ID_LEAD_PILOT, serial_udb_extra_f16->sue_ID_DIY_DRONES_URL); +} + +/** * @brief Send a serial_udb_extra_f16 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h index ddfc236ba..15ba68a34 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_a message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_status Serial UDB Extra Status @@ -305,7 +305,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_a struct into a message + * @brief Encode a serial_udb_extra_f2_a struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -318,6 +318,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode(uint8_t system_i } /** + * @brief Encode a serial_udb_extra_f2_a struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_a C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_a_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_a_t* serial_udb_extra_f2_a) +{ + return mavlink_msg_serial_udb_extra_f2_a_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_a->sue_time, serial_udb_extra_f2_a->sue_status, serial_udb_extra_f2_a->sue_latitude, serial_udb_extra_f2_a->sue_longitude, serial_udb_extra_f2_a->sue_altitude, serial_udb_extra_f2_a->sue_waypoint_index, serial_udb_extra_f2_a->sue_rmat0, serial_udb_extra_f2_a->sue_rmat1, serial_udb_extra_f2_a->sue_rmat2, serial_udb_extra_f2_a->sue_rmat3, serial_udb_extra_f2_a->sue_rmat4, serial_udb_extra_f2_a->sue_rmat5, serial_udb_extra_f2_a->sue_rmat6, serial_udb_extra_f2_a->sue_rmat7, serial_udb_extra_f2_a->sue_rmat8, serial_udb_extra_f2_a->sue_cog, serial_udb_extra_f2_a->sue_sog, serial_udb_extra_f2_a->sue_cpu_load, serial_udb_extra_f2_a->sue_voltage_milis, serial_udb_extra_f2_a->sue_air_speed_3DIMU, serial_udb_extra_f2_a->sue_estimated_wind_0, serial_udb_extra_f2_a->sue_estimated_wind_1, serial_udb_extra_f2_a->sue_estimated_wind_2, serial_udb_extra_f2_a->sue_magFieldEarth0, serial_udb_extra_f2_a->sue_magFieldEarth1, serial_udb_extra_f2_a->sue_magFieldEarth2, serial_udb_extra_f2_a->sue_svs, serial_udb_extra_f2_a->sue_hdop); +} + +/** * @brief Send a serial_udb_extra_f2_a message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h index 74da8416d..7cb8c87da 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h @@ -219,7 +219,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack(uint8_t system_id, * @brief Pack a serial_udb_extra_f2_b message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_time Serial UDB Extra Time * @param sue_pwm_input_1 Serial UDB Extra PWM Input Channel 1 @@ -345,7 +345,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_pack_chan(uint8_t syste } /** - * @brief Encode a serial_udb_extra_f2_b struct into a message + * @brief Encode a serial_udb_extra_f2_b struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -358,6 +358,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode(uint8_t system_i } /** + * @brief Encode a serial_udb_extra_f2_b struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f2_b C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f2_b_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f2_b_t* serial_udb_extra_f2_b) +{ + return mavlink_msg_serial_udb_extra_f2_b_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f2_b->sue_time, serial_udb_extra_f2_b->sue_pwm_input_1, serial_udb_extra_f2_b->sue_pwm_input_2, serial_udb_extra_f2_b->sue_pwm_input_3, serial_udb_extra_f2_b->sue_pwm_input_4, serial_udb_extra_f2_b->sue_pwm_input_5, serial_udb_extra_f2_b->sue_pwm_input_6, serial_udb_extra_f2_b->sue_pwm_input_7, serial_udb_extra_f2_b->sue_pwm_input_8, serial_udb_extra_f2_b->sue_pwm_input_9, serial_udb_extra_f2_b->sue_pwm_input_10, serial_udb_extra_f2_b->sue_pwm_output_1, serial_udb_extra_f2_b->sue_pwm_output_2, serial_udb_extra_f2_b->sue_pwm_output_3, serial_udb_extra_f2_b->sue_pwm_output_4, serial_udb_extra_f2_b->sue_pwm_output_5, serial_udb_extra_f2_b->sue_pwm_output_6, serial_udb_extra_f2_b->sue_pwm_output_7, serial_udb_extra_f2_b->sue_pwm_output_8, serial_udb_extra_f2_b->sue_pwm_output_9, serial_udb_extra_f2_b->sue_pwm_output_10, serial_udb_extra_f2_b->sue_imu_location_x, serial_udb_extra_f2_b->sue_imu_location_y, serial_udb_extra_f2_b->sue_imu_location_z, serial_udb_extra_f2_b->sue_flags, serial_udb_extra_f2_b->sue_osc_fails, serial_udb_extra_f2_b->sue_imu_velocity_x, serial_udb_extra_f2_b->sue_imu_velocity_y, serial_udb_extra_f2_b->sue_imu_velocity_z, serial_udb_extra_f2_b->sue_waypoint_goal_x, serial_udb_extra_f2_b->sue_waypoint_goal_y, serial_udb_extra_f2_b->sue_waypoint_goal_z, serial_udb_extra_f2_b->sue_memory_stack_free); +} + +/** * @brief Send a serial_udb_extra_f2_b message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h index 83ccbbedb..77c616cc2 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f4 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_ROLL_STABILIZATION_AILERONS Serial UDB Extra Roll Stabilization with Ailerons Enabled * @param sue_ROLL_STABILIZATION_RUDDER Serial UDB Extra Roll Stabilization with Rudder Enabled @@ -161,7 +161,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f4 struct into a message + * @brief Encode a serial_udb_extra_f4 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -174,6 +174,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f4 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f4 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f4_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f4_t* serial_udb_extra_f4) +{ + return mavlink_msg_serial_udb_extra_f4_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f4->sue_ROLL_STABILIZATION_AILERONS, serial_udb_extra_f4->sue_ROLL_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_PITCH_STABILIZATION, serial_udb_extra_f4->sue_YAW_STABILIZATION_RUDDER, serial_udb_extra_f4->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f4->sue_AILERON_NAVIGATION, serial_udb_extra_f4->sue_RUDDER_NAVIGATION, serial_udb_extra_f4->sue_ALTITUDEHOLD_STABILIZED, serial_udb_extra_f4->sue_ALTITUDEHOLD_WAYPOINT, serial_udb_extra_f4->sue_RACING_MODE); +} + +/** * @brief Send a serial_udb_extra_f4 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h index 2b2451f00..e115a9b44 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f5 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_AILERON Serial UDB YAWKP_AILERON Gain for Proporional control of navigation * @param sue_YAWKD_AILERON Serial UDB YAWKD_AILERON Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f5 struct into a message + * @brief Encode a serial_udb_extra_f5 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f5 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f5 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f5_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f5_t* serial_udb_extra_f5) +{ + return mavlink_msg_serial_udb_extra_f5_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f5->sue_YAWKP_AILERON, serial_udb_extra_f5->sue_YAWKD_AILERON, serial_udb_extra_f5->sue_ROLLKP, serial_udb_extra_f5->sue_ROLLKD, serial_udb_extra_f5->sue_YAW_STABILIZATION_AILERON, serial_udb_extra_f5->sue_AILERON_BOOST); +} + +/** * @brief Send a serial_udb_extra_f5 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h index 9d58ca9a8..656103d09 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f6 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_PITCHGAIN Serial UDB Extra PITCHGAIN Proportional Control * @param sue_PITCHKD Serial UDB Extra Pitch Rate Control @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f6 struct into a message + * @brief Encode a serial_udb_extra_f6 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f6 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f6 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f6_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f6_t* serial_udb_extra_f6) +{ + return mavlink_msg_serial_udb_extra_f6_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f6->sue_PITCHGAIN, serial_udb_extra_f6->sue_PITCHKD, serial_udb_extra_f6->sue_RUDDER_ELEV_MIX, serial_udb_extra_f6->sue_ROLL_ELEV_MIX, serial_udb_extra_f6->sue_ELEVATOR_BOOST); +} + +/** * @brief Send a serial_udb_extra_f6 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h index ab73b967e..51c98e6b7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f7 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_YAWKP_RUDDER Serial UDB YAWKP_RUDDER Gain for Proporional control of navigation * @param sue_YAWKD_RUDDER Serial UDB YAWKD_RUDDER Gain for Rate control of navigation @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f7 struct into a message + * @brief Encode a serial_udb_extra_f7 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f7 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f7 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f7_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f7_t* serial_udb_extra_f7) +{ + return mavlink_msg_serial_udb_extra_f7_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f7->sue_YAWKP_RUDDER, serial_udb_extra_f7->sue_YAWKD_RUDDER, serial_udb_extra_f7->sue_ROLLKP_RUDDER, serial_udb_extra_f7->sue_ROLLKD_RUDDER, serial_udb_extra_f7->sue_RUDDER_BOOST, serial_udb_extra_f7->sue_RTL_PITCH_DOWN); +} + +/** * @brief Send a serial_udb_extra_f7 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h index 310246e8e..8557a95e2 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack(uint8_t system_id, u * @brief Pack a serial_udb_extra_f8 message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param sue_HEIGHT_TARGET_MAX Serial UDB Extra HEIGHT_TARGET_MAX * @param sue_HEIGHT_TARGET_MIN Serial UDB Extra HEIGHT_TARGET_MIN @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_pack_chan(uint8_t system_ } /** - * @brief Encode a serial_udb_extra_f8 struct into a message + * @brief Encode a serial_udb_extra_f8 struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode(uint8_t system_id, } /** + * @brief Encode a serial_udb_extra_f8 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_udb_extra_f8 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_udb_extra_f8_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_udb_extra_f8_t* serial_udb_extra_f8) +{ + return mavlink_msg_serial_udb_extra_f8_pack_chan(system_id, component_id, chan, msg, serial_udb_extra_f8->sue_HEIGHT_TARGET_MAX, serial_udb_extra_f8->sue_HEIGHT_TARGET_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MIN, serial_udb_extra_f8->sue_ALT_HOLD_THROTTLE_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MIN, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_MAX, serial_udb_extra_f8->sue_ALT_HOLD_PITCH_HIGH); +} + +/** * @brief Send a serial_udb_extra_f8 message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index ea0265298..59f3d2ded 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:50 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:51 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h index d89363577..51afac87c 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h +++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h @@ -9,6 +9,10 @@ #endif #include <math.h> +#ifndef M_PI_2 + #define M_PI_2 ((float)asin(1)) +#endif + /** * @file mavlink_conversions.h * @@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo float phi, theta, psi; theta = asin(-dcm[2][0]); - if (fabs(theta - M_PI_2) < 1.0e-3f) { + if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; - psi = (atan2(dcm[1][2] - dcm[0][1], + psi = (atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1]) + phi); - } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) { phi = 0.0f; psi = atan2f(dcm[1][2] - dcm[0][1], dcm[0][2] + dcm[1][1] - phi); @@ -128,4 +132,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo dcm[2][2] = cosPhi * cosThe; } -#endif
\ No newline at end of file +#endif diff --git a/mavlink/include/mavlink/v1.0/mavlink_helpers.h b/mavlink/include/mavlink/v1.0/mavlink_helpers.h index 72cf91fb9..96672f847 100644 --- a/mavlink/include/mavlink/v1.0/mavlink_helpers.h +++ b/mavlink/include/mavlink/v1.0/mavlink_helpers.h @@ -16,7 +16,12 @@ #ifndef MAVLINK_GET_CHANNEL_STATUS MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { +#if MAVLINK_EXTERNAL_RX_STATUS + // No m_mavlink_status array defined in function, + // has to be defined externally +#else static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#endif return &m_mavlink_status[chan]; } #endif @@ -29,11 +34,8 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { #if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_message array defined in function, + // No m_mavlink_buffer array defined in function, // has to be defined externally -#ifndef m_mavlink_message -#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];) -#endif #else static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; #endif diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h index 1aac8395d..ef5354d5e 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h @@ -99,7 +99,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint * @brief Pack a attitude_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target The system to be controlled * @param roll roll @@ -153,7 +153,7 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, } /** - * @brief Encode a attitude_control struct into a message + * @brief Encode a attitude_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -166,6 +166,20 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui } /** + * @brief Encode a attitude_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param attitude_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_control_t* attitude_control) +{ + return mavlink_msg_attitude_control_pack_chan(system_id, component_id, chan, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); +} + +/** * @brief Send a attitude_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h index 353e10581..d41093792 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @brief Pack a brief_feature message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param x x position in m * @param y y position in m @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui } /** - * @brief Encode a brief_feature struct into a message + * @brief Encode a brief_feature struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -154,6 +154,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 } /** + * @brief Encode a brief_feature struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param brief_feature C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_brief_feature_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) +{ + return mavlink_msg_brief_feature_pack_chan(system_id, component_id, chan, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); +} + +/** * @brief Send a brief_feature message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h index 5251c35c3..a7af8c8eb 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst * @brief Pack a data_transmission_handshake message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) * @param size total data size in bytes (set on ACK only) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t } /** - * @brief Encode a data_transmission_handshake struct into a message + * @brief Encode a data_transmission_handshake struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy } /** + * @brief Encode a data_transmission_handshake struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param data_transmission_handshake C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_data_transmission_handshake_t* data_transmission_handshake) +{ + return mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, chan, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->width, data_transmission_handshake->height, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); +} + +/** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h index feeef6f39..40001dc30 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin * @brief Pack a encapsulated_data message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes @@ -93,7 +93,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id } /** - * @brief Encode a encapsulated_data struct into a message + * @brief Encode a encapsulated_data struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -106,6 +106,20 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u } /** + * @brief Encode a encapsulated_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param encapsulated_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_encapsulated_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_encapsulated_data_t* encapsulated_data) +{ + return mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, chan, msg, encapsulated_data->seqnr, encapsulated_data->data); +} + +/** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h index 6d1d9cca7..ae4db825d 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h @@ -169,7 +169,7 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 * @brief Pack a image_available message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_id Camera id * @param cam_no Camera # (starts with 0) @@ -265,7 +265,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_available struct into a message + * @brief Encode a image_available struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -278,6 +278,20 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin } /** + * @brief Encode a image_available struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_available C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_available_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_available_t* image_available) +{ + return mavlink_msg_image_available_pack_chan(system_id, component_id, chan, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); +} + +/** * @brief Send a image_available message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h index 784cedf8b..664574be9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h @@ -59,7 +59,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, * @brief Pack a image_trigger_control message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param enable 0 to disable, 1 to enable * @return length of the message in bytes (excluding serial stream start sign) @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste } /** - * @brief Encode a image_trigger_control struct into a message + * @brief Encode a image_trigger_control struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -102,6 +102,20 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i } /** + * @brief Encode a image_trigger_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_trigger_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_trigger_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_trigger_control_t* image_trigger_control) +{ + return mavlink_msg_image_trigger_control_pack_chan(system_id, component_id, chan, msg, image_trigger_control->enable); +} + +/** * @brief Send a image_trigger_control message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h index 05b0d775f..f3a2243af 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h @@ -114,7 +114,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 * @brief Pack a image_triggered message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param timestamp Timestamp * @param seq IMU seq @@ -177,7 +177,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, } /** - * @brief Encode a image_triggered struct into a message + * @brief Encode a image_triggered struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -190,6 +190,20 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin } /** + * @brief Encode a image_triggered struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param image_triggered C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_triggered_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered) +{ + return mavlink_msg_image_triggered_pack_chan(system_id, component_id, chan, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); +} + +/** * @brief Send a image_triggered message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h index 817ec60a2..6bdcceaf9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon * @brief Pack a marker message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID * @param x x position @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c } /** - * @brief Encode a marker struct into a message + * @brief Encode a marker struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp } /** + * @brief Encode a marker struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param marker C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_marker_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_marker_t* marker) +{ + return mavlink_msg_marker_pack_chan(system_id, component_id, chan, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); +} + +/** * @brief Send a marker message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h index 7e29d7152..aa7b827a3 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @brief Pack a pattern_detected message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Pattern, 1: Letter * @param confidence Confidence of detection @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, } /** - * @brief Encode a pattern_detected struct into a message + * @brief Encode a pattern_detected struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui } /** + * @brief Encode a pattern_detected struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pattern_detected C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pattern_detected_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pattern_detected_t* pattern_detected) +{ + return mavlink_msg_pattern_detected_pack_chan(system_id, component_id, chan, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); +} + +/** * @brief Send a pattern_detected message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h index a6faebbb5..913a52897 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h @@ -92,7 +92,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @brief Pack a point_of_interest message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -141,7 +141,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id } /** - * @brief Encode a point_of_interest struct into a message + * @brief Encode a point_of_interest struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -154,6 +154,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u } /** + * @brief Encode a point_of_interest struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_t* point_of_interest) +{ + return mavlink_msg_point_of_interest_pack_chan(system_id, component_id, chan, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); +} + +/** * @brief Send a point_of_interest message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h index 8d02f9e5c..e24436431 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -107,7 +107,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @brief Pack a point_of_interest_connection message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta @@ -165,7 +165,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ } /** - * @brief Encode a point_of_interest_connection struct into a message + * @brief Encode a point_of_interest_connection struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -178,6 +178,20 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s } /** + * @brief Encode a point_of_interest_connection struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param point_of_interest_connection C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_point_of_interest_connection_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_point_of_interest_connection_t* point_of_interest_connection) +{ + return mavlink_msg_point_of_interest_connection_pack_chan(system_id, component_id, chan, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); +} + +/** * @brief Send a point_of_interest_connection message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h index 6c86be3ab..6f4ca510a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system * @brief Pack a position_control_setpoint message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param id ID of waypoint, 0 for plain position * @param x x position @@ -121,7 +121,7 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s } /** - * @brief Encode a position_control_setpoint struct into a message + * @brief Encode a position_control_setpoint struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -134,6 +134,20 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst } /** + * @brief Encode a position_control_setpoint struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param position_control_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_control_setpoint_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_position_control_setpoint_t* position_control_setpoint) +{ + return mavlink_msg_position_control_setpoint_pack_chan(system_id, component_id, chan, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); +} + +/** * @brief Send a position_control_setpoint message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h index 0a0dbdb37..08cedbb4b 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h @@ -89,7 +89,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo * @brief Pack a raw_aux message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a raw_aux struct into a message + * @brief Encode a raw_aux struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a raw_aux struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_aux C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_aux_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_aux_t* raw_aux) +{ + return mavlink_msg_raw_aux_pack_chan(system_id, component_id, chan, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); +} + +/** * @brief Send a raw_aux message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h index 7be640911..b26d748c2 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 * @brief Pack a set_cam_shutter message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param cam_no Camera id * @param cam_mode Camera mode: 0 = auto, 1 = manual @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, } /** - * @brief Encode a set_cam_shutter struct into a message + * @brief Encode a set_cam_shutter struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin } /** + * @brief Encode a set_cam_shutter struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_cam_shutter C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_cam_shutter_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_cam_shutter_t* set_cam_shutter) +{ + return mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, chan, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); +} + +/** * @brief Send a set_cam_shutter message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h index 25bff659e..f1f9e698f 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack(uint8_t syst * @brief Pack a set_position_control_offset message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_set_position_control_offset_pack_chan(uint8_t } /** - * @brief Encode a set_position_control_offset struct into a message + * @brief Encode a set_position_control_offset struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_set_position_control_offset_encode(uint8_t sy } /** + * @brief Encode a set_position_control_offset struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param set_position_control_offset C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_position_control_offset_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_position_control_offset_t* set_position_control_offset) +{ + return mavlink_msg_set_position_control_offset_pack_chan(system_id, component_id, chan, msg, set_position_control_offset->target_system, set_position_control_offset->target_component, set_position_control_offset->x, set_position_control_offset->y, set_position_control_offset->z, set_position_control_offset->yaw); +} + +/** * @brief Send a set_position_control_offset message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h index 426788018..9458087da 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h @@ -74,7 +74,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint * @brief Pack a watchdog_command message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target_system_id Target system ID * @param watchdog_id Watchdog ID @@ -113,7 +113,7 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, } /** - * @brief Encode a watchdog_command struct into a message + * @brief Encode a watchdog_command struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -126,6 +126,20 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui } /** + * @brief Encode a watchdog_command struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_command C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_command_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_command_t* watchdog_command) +{ + return mavlink_msg_watchdog_command_pack_chan(system_id, component_id, chan, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); +} + +/** * @brief Send a watchdog_command message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h index 49478999c..3f4295ee5 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -64,7 +64,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui * @brief Pack a watchdog_heartbeat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_count Number of processes @@ -97,7 +97,7 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i } /** - * @brief Encode a watchdog_heartbeat struct into a message + * @brief Encode a watchdog_heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -110,6 +110,20 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, } /** + * @brief Encode a watchdog_heartbeat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_heartbeat_t* watchdog_heartbeat) +{ + return mavlink_msg_watchdog_heartbeat_pack_chan(system_id, component_id, chan, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); +} + +/** * @brief Send a watchdog_heartbeat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h index d706ef85e..55853cdde 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h @@ -78,7 +78,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @brief Pack a watchdog_process_info message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste } /** - * @brief Encode a watchdog_process_info struct into a message + * @brief Encode a watchdog_process_info struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -131,6 +131,20 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i } /** + * @brief Encode a watchdog_process_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_info_t* watchdog_process_info) +{ + return mavlink_msg_watchdog_process_info_pack_chan(system_id, component_id, chan, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); +} + +/** * @brief Send a watchdog_process_info message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h index b1bbaaae7..a0410d803 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h @@ -84,7 +84,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i * @brief Pack a watchdog_process_status message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param watchdog_id Watchdog ID * @param process_id Process ID @@ -129,7 +129,7 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys } /** - * @brief Encode a watchdog_process_status struct into a message + * @brief Encode a watchdog_process_status struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -142,6 +142,20 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system } /** + * @brief Encode a watchdog_process_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param watchdog_process_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_watchdog_process_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_watchdog_process_status_t* watchdog_process_status) +{ + return mavlink_msg_watchdog_process_status_pack_chan(system_id, component_id, chan, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); +} + +/** * @brief Send a watchdog_process_status message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index a2d0d0e14..3fc9e6477 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -72,6 +72,7 @@ enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ @@ -80,7 +81,8 @@ enum MAV_CMD MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_ENUM_END=401, /* | */ + MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + MAV_CMD_ENUM_END=501, /* | */ MAV_CMD_DO_START_SEARCH=10001, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_DO_FINISH_SEARCH=10002, /* Starts a search |1 to arm, 0 to disarm| */ MAV_CMD_NAV_SWEEP=10003, /* Starts a search |1 to arm, 0 to disarm| */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index 61fe6e930..07831a425 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:05 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:49:13 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/protocol.h b/mavlink/include/mavlink/v1.0/protocol.h index ddacae59c..86e7ff588 100644 --- a/mavlink/include/mavlink/v1.0/protocol.h +++ b/mavlink/include/mavlink/v1.0/protocol.h @@ -162,7 +162,7 @@ static inline void byte_copy_8(char *dst, const char *src) /* like memcpy(), but if src is NULL, do a memset to zero */ -static void mav_array_memcpy(void *dest, const void *src, size_t n) +static inline void mav_array_memcpy(void *dest, const void *src, size_t n) { if (src == NULL) { memset(dest, 0, n); diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h index 5cdd58456..73e9d75db 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack(uint8_t system_id, uint * @brief Pack a cmd_airspeed_ack message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param spCmd @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, } /** - * @brief Encode a cmd_airspeed_ack struct into a message + * @brief Encode a cmd_airspeed_ack struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, ui } /** + * @brief Encode a cmd_airspeed_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) +{ + return mavlink_msg_cmd_airspeed_ack_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); +} + +/** * @brief Send a cmd_airspeed_ack message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h index 2a4894fe7..b6b567f99 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uin * @brief Pack a cmd_airspeed_chng message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param target @@ -109,7 +109,7 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id } /** - * @brief Encode a cmd_airspeed_chng struct into a message + * @brief Encode a cmd_airspeed_chng struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -122,6 +122,20 @@ static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, u } /** + * @brief Encode a cmd_airspeed_chng struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cmd_airspeed_chng C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) +{ + return mavlink_msg_cmd_airspeed_chng_pack_chan(system_id, component_id, chan, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); +} + +/** * @brief Send a cmd_airspeed_chng message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h index d600e9174..642f7aab9 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t * @brief Pack a filt_rot_vel message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param rotVel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a filt_rot_vel struct into a message + * @brief Encode a filt_rot_vel struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a filt_rot_vel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param filt_rot_vel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_filt_rot_vel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) +{ + return mavlink_msg_filt_rot_vel_pack_chan(system_id, component_id, chan, msg, filt_rot_vel->rotVel); +} + +/** * @brief Send a filt_rot_vel message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h index cb1c86f43..0f8369881 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t compo * @brief Pack a llc_out message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param servoOut @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a llc_out struct into a message + * @brief Encode a llc_out struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -123,6 +123,20 @@ static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a llc_out struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param llc_out C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_llc_out_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) +{ + return mavlink_msg_llc_out_pack_chan(system_id, component_id, chan, msg, llc_out->servoOut, llc_out->MotorOut); +} + +/** * @brief Send a llc_out message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h index b6bd0d9e5..5d9382326 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t * @brief Pack a obs_air_temp message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param airT @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_air_temp struct into a message + * @brief Encode a obs_air_temp struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_air_temp struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_temp C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_temp_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) +{ + return mavlink_msg_obs_air_temp_pack_chan(system_id, component_id, chan, msg, obs_air_temp->airT); +} + +/** * @brief Send a obs_air_temp message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h index 87a65cfa0..35d813ca1 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint * @brief Pack a obs_air_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param magnitude @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, } /** - * @brief Encode a obs_air_velocity struct into a message + * @brief Encode a obs_air_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -136,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, ui } /** + * @brief Encode a obs_air_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_air_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_air_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) +{ + return mavlink_msg_obs_air_velocity_pack_chan(system_id, component_id, chan, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); +} + +/** * @brief Send a obs_air_velocity message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h index 602eafc80..9c80cd66e 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t * @brief Pack a obs_attitude message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param quat @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_attitude struct into a message + * @brief Encode a obs_attitude struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) +{ + return mavlink_msg_obs_attitude_pack_chan(system_id, component_id, chan, msg, obs_attitude->quat); +} + +/** * @brief Send a obs_attitude message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h index 3ab855ba8..24dd43b57 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h @@ -73,7 +73,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_bias message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param accBias @@ -110,7 +110,7 @@ static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_bias struct into a message + * @brief Encode a obs_bias struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -123,6 +123,20 @@ static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a obs_bias struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_bias C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_bias_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) +{ + return mavlink_msg_obs_bias_pack_chan(system_id, component_id, chan, msg, obs_bias->accBias, obs_bias->gyroBias); +} + +/** * @brief Send a obs_bias message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h index ec494d51a..cfc2fe7e1 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h @@ -81,7 +81,7 @@ static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t * @brief Pack a obs_position message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param lon @@ -123,7 +123,7 @@ static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_position struct into a message + * @brief Encode a obs_position struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -136,6 +136,20 @@ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_position struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) +{ + return mavlink_msg_obs_position_pack_chan(system_id, component_id, chan, msg, obs_position->lon, obs_position->lat, obs_position->alt); +} + +/** * @brief Send a obs_position message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h index e42b0261f..24e272bf7 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t compo * @brief Pack a obs_qff message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param qff @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_qff struct into a message + * @brief Encode a obs_qff struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a obs_qff struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_qff C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_qff_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) +{ + return mavlink_msg_obs_qff_pack_chan(system_id, component_id, chan, msg, obs_qff->qff); +} + +/** * @brief Send a obs_qff message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h index 2fa6e9111..6e3776632 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t * @brief Pack a obs_velocity message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param vel @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uin } /** - * @brief Encode a obs_velocity struct into a message + * @brief Encode a obs_velocity struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_ } /** + * @brief Encode a obs_velocity struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_velocity C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_velocity_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) +{ + return mavlink_msg_obs_velocity_pack_chan(system_id, component_id, chan, msg, obs_velocity->vel); +} + +/** * @brief Send a obs_velocity message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h index bc9f6e4ef..55f7cb2ae 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h @@ -63,7 +63,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t comp * @brief Pack a obs_wind message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param wind @@ -95,7 +95,7 @@ static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a obs_wind struct into a message + * @brief Encode a obs_wind struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -108,6 +108,20 @@ static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a obs_wind struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obs_wind C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obs_wind_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) +{ + return mavlink_msg_obs_wind_pack_chan(system_id, component_id, chan, msg, obs_wind->wind); +} + +/** * @brief Send a obs_wind message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h index 0799af07f..e0963ece7 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t compo * @brief Pack a pm_elec message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param PwCons @@ -119,7 +119,7 @@ static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a pm_elec struct into a message + * @brief Encode a pm_elec struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -132,6 +132,20 @@ static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t com } /** + * @brief Encode a pm_elec struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param pm_elec C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_pm_elec_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) +{ + return mavlink_msg_pm_elec_pack_chan(system_id, component_id, chan, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); +} + +/** * @brief Send a pm_elec message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h index e5e483a73..94f3e58bb 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h @@ -90,7 +90,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t comp * @brief Pack a sys_stat message on a channel * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over + * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param gps @@ -137,7 +137,7 @@ static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t } /** - * @brief Encode a sys_stat struct into a message + * @brief Encode a sys_stat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -150,6 +150,20 @@ static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t co } /** + * @brief Encode a sys_stat struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param sys_stat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_stat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) +{ + return mavlink_msg_sys_stat_pack_chan(system_id, component_id, chan, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); +} + +/** * @brief Send a sys_stat message * @param chan MAVLink channel to send the message * diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index 4633e6eb5..ab3f47b9d 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:22 2013" +#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:02 2013" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 5be528f8c..e43b9c18e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -327,7 +327,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=196608 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_INTERRUPTSTACK=4096 # # Boot options @@ -701,18 +701,18 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=4000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set # # Basic CXX Support # -# CONFIG_C99_BOOL8 is not set +CONFIG_C99_BOOL8=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y # CONFIG_CXX_NEWLONG is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0615950a2..372453fb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -38,7 +38,31 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -# CONFIG_DEBUG is not set +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set CONFIG_DEBUG_SYMBOLS=y # @@ -86,6 +110,7 @@ CONFIG_ARCH_HAVE_FPU=y CONFIG_ARCH_FPU=y CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set # # ARMV7M Configuration Options @@ -94,7 +119,9 @@ CONFIG_ARCH_HAVE_MPU=y CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set # # STM32 Configuration Options @@ -240,9 +267,6 @@ CONFIG_STM32_ADC=y CONFIG_STM32_SPI=y CONFIG_STM32_I2C=y -CONFIG_ARCH_HAVE_LEDS=y -# CONFIG_ARCH_LEDS is not set - # # Alternate Pin Mapping # @@ -254,7 +278,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=y +# CONFIG_STM32_CCMEXCLUDE is not set CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set @@ -271,40 +295,22 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -CONFIG_USART1_RXDMA=y +# CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set # CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RS485 is not set # CONFIG_UART4_RXDMA is not set -# CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set -# CONFIG_USART7_RXDMA is not set -CONFIG_USART8_RXDMA=y +# CONFIG_UART7_RS485 is not set +# CONFIG_UART7_RXDMA is not set +# CONFIG_UART8_RS485 is not set +# CONFIG_UART8_RXDMA is not set +CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y - -# Previous: -## CONFIG_USART1_RS485 is not set -#CONFIG_USART1_RXDMA=y -## CONFIG_USART2_RS485 is not set -#CONFIG_USART2_RXDMA=y -## CONFIG_USART3_RS485 is not set -#CONFIG_USART3_RXDMA=y -## CONFIG_UART4_RS485 is not set -#CONFIG_UART4_RXDMA=y -## CONFIG_UART5_RXDMA is not set -## CONFIG_USART6_RS485 is not set -## CONFIG_USART6_RXDMA is not set -## CONFIG_USART7_RXDMA is not set -#CONFIG_USART8_RXDMA=y -#CONFIG_STM32_USART_SINGLEWIRE=y - - - - # # SPI Configuration # @@ -323,25 +329,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # # SDIO Configuration # - -# -# Maintain legacy build behavior (revisit) -# - -CONFIG_MMCSD=y -#CONFIG_MMCSD_SPI=y -CONFIG_MMCSD_SDIO=y -CONFIG_MTD=y - -# -# STM32 SDIO-based MMC/SD driver -# -CONFIG_SDIO_DMA=y -# CONFIG_SDIO_DMAPRIO is not set -# CONFIG_SDIO_WIDTH_D1_ONLY is not set -CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -CONFIG_MMCSD_MMCSUPPORT=n -CONFIG_MMCSD_HAVECARDDETECT=n +CONFIG_SDIO_PRI=128 # # USB Host Configuration @@ -379,7 +367,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_INTERRUPTSTACK=4096 # # Boot options @@ -393,15 +381,14 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V2=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="px4fmu-v2" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" # # Common Board Options # -CONFIG_NSH_MMCSDSLOTNO=0 CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 # # Board-Specific Options @@ -423,7 +410,7 @@ CONFIG_START_DAY=1 CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y -CONFIG_SEM_PREALLOCHOLDERS=8 +CONFIG_SEM_PREALLOCHOLDERS=0 CONFIG_SEM_NNESTPRIO=8 # CONFIG_FDCLONE_DISABLE is not set CONFIG_FDCLONE_STDIO=y @@ -497,7 +484,16 @@ CONFIG_WATCHDOG=y # CONFIG_BCH is not set # CONFIG_INPUT is not set # CONFIG_LCD is not set -# CONFIG_MMCSD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set CONFIG_MTD=y # @@ -523,7 +519,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -536,6 +532,7 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set # CONFIG_USART1_SERIAL_CONSOLE is not set # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_USART3_SERIAL_CONSOLE is not set @@ -640,7 +637,6 @@ CONFIG_USBDEV=y # CONFIG_USBDEV_SELFPOWERED is not set CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 -# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set @@ -650,7 +646,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +# CONFIG_CDCACM_CONSOLE is not set CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -702,7 +698,7 @@ CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y -# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FAT_DMAMEMORY=y CONFIG_FS_NXFFS=y CONFIG_NXFFS_PREALLOCATED=y CONFIG_NXFFS_ERASEDSTATE=0xff @@ -716,10 +712,8 @@ CONFIG_FS_BINFS=y # # System Logging # -CONFIG_SYSLOG_ENABLE=y -CONFIG_SYSLOG=y -CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set # # Graphics Support @@ -733,8 +727,9 @@ CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # CONFIG_MM_SMALL is not set CONFIG_MM_REGIONS=2 CONFIG_GRAN=y -CONFIG_GRAN_SINGLE=y -CONFIG_GRAN_INTR=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set # # Audio Support @@ -800,11 +795,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=2048 +CONFIG_SCHED_WORKSTACKSIZE=4000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=2048 +CONFIG_SCHED_LPWORKSTACKSIZE=4000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set @@ -1002,6 +997,7 @@ CONFIG_NSH_CONSOLE=y # # USB Trace Support # +# CONFIG_NSH_USBDEV_TRACE is not set # CONFIG_NSH_CONDEV is not set CONFIG_NSH_ARCHINIT=y @@ -1031,6 +1027,7 @@ CONFIG_NSH_ARCHINIT=y # # FLASH Erase-all Command # +# CONFIG_SYSTEM_FLASH_ERASEALL is not set # # readline() diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 1ec61eb60..5e45cc936 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -77,10 +78,9 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), + _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), + _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), @@ -88,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _airspeed_pub(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), - _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")) + _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = true; @@ -105,7 +104,7 @@ Airspeed::~Airspeed() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -123,20 +122,14 @@ Airspeed::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct differential_pressure_s[_num_reports]; - - for (unsigned i = 0; i < _num_reports; i++) - _reports[i].max_differential_pressure_pa = 0; - + _reports = new RingBuffer(2, sizeof(differential_pressure_s)); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the airspeed topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &_reports[0]); + differential_pressure_s zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report); if (_airspeed_pub < 0) warnx("failed to create airspeed sensor object. Did you start uOrb?"); @@ -229,31 +222,22 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct differential_pressure_s *buf = new struct differential_pressure_s[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -281,7 +265,8 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t Airspeed::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct differential_pressure_s); + unsigned count = buflen / sizeof(differential_pressure_s); + differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -297,10 +282,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(abuf)) { + ret += sizeof(*abuf); + abuf++; } } @@ -309,9 +293,8 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -329,8 +312,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(abuf)) { + ret = sizeof(*abuf); + } } while (0); @@ -342,7 +326,7 @@ Airspeed::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); @@ -385,6 +369,12 @@ Airspeed::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); warnx("poll interval: %u ticks", _measure_ticks); - warnx("report queue: %u (%u/%u @ %p)", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); +} + +void +Airspeed::new_report(const differential_pressure_s &report) +{ + if (!_reports->force(&report)) + perf_count(_buffer_overflows); } diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index b87494b40..048784813 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -102,6 +103,10 @@ public: */ virtual void print_info(); +private: + RingBuffer *_reports; + perf_counter_t _buffer_overflows; + protected: virtual int probe(); @@ -114,10 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; + uint16_t _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -129,7 +131,6 @@ protected: perf_counter_t _sample_perf; perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; /** @@ -162,8 +163,12 @@ protected: */ static void cycle_trampoline(void *arg); + /** + * add a new report to the reports queue + * + * @param report differential_pressure_s report + */ + void new_report(const differential_pressure_s &report); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index 079b5d21c..1590cc182 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -63,6 +63,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_accel.h> +#include <drivers/device/ringbuffer.h> /* oddly, ERROR is not defined for c++ */ @@ -146,10 +147,7 @@ private: struct hrt_call _call; unsigned _call_interval; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct accel_report *_reports; + RingBuffer *_reports; struct accel_scale _accel_scale; float _accel_range_scale; @@ -233,16 +231,9 @@ private: int set_lowpass(unsigned frequency); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - BMA180::BMA180(int bus, spi_dev_e device) : SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -270,7 +261,7 @@ BMA180::~BMA180() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -286,16 +277,15 @@ BMA180::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct accel_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(accel_report)); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); /* perform soft reset (p48) */ write_reg(ADDR_RESET, SOFT_RESET); @@ -352,6 +342,7 @@ ssize_t BMA180::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -367,10 +358,9 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(arp)) { + ret += sizeof(*arp); + arp++; } } @@ -379,12 +369,12 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(arp)) + ret = sizeof(*arp); return ret; } @@ -449,31 +439,22 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement */ @@ -654,7 +635,7 @@ BMA180::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this); @@ -688,7 +669,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - accel_report *report = &_reports[_next_report]; + struct accel_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -708,45 +689,41 @@ BMA180::measure() * them before. There is no good way to synchronise with the internal * measurement flow without using the external interrupt. */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. * Two non-value bits are discarded directly */ - report->y_raw = read_reg(ADDR_ACC_X_LSB + 0); - report->y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; - report->x_raw = read_reg(ADDR_ACC_X_LSB + 2); - report->x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; - report->z_raw = read_reg(ADDR_ACC_X_LSB + 4); - report->z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; + report.y_raw = read_reg(ADDR_ACC_X_LSB + 0); + report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8; + report.x_raw = read_reg(ADDR_ACC_X_LSB + 2); + report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8; + report.z_raw = read_reg(ADDR_ACC_X_LSB + 4); + report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8; /* discard two non-value bits in the 16 bit measurement */ - report->x_raw = (report->x_raw / 4); - report->y_raw = (report->y_raw / 4); - report->z_raw = (report->z_raw / 4); + report.x_raw = (report.x_raw / 4); + report.y_raw = (report.y_raw / 4); + report.z_raw = (report.z_raw / 4); /* invert y axis, due to 14 bit data no overflow can occur in the negation */ - report->y_raw = -report->y_raw; - - report->x = ((report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - report->y = ((report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - report->z = ((report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - report->scaling = _accel_range_scale; - report->range_m_s2 = _accel_range_m_s2; + report.y_raw = -report.y_raw; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + report.scaling = _accel_range_scale; + report.range_m_s2 = _accel_range_m_s2; - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &report); /* stop the perf counter */ perf_end(_sample_perf); @@ -756,8 +733,7 @@ void BMA180::print_info() { perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 135767b26..ae2a645f7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -58,6 +58,7 @@ #include <nuttx/sdio.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> +#include <nuttx/gran.h> #include <stm32.h> #include "board_config.h" @@ -69,6 +70,7 @@ #include <drivers/drv_led.h> #include <systemlib/cpuload.h> +#include <systemlib/perf_counter.h> /**************************************************************************** * Pre-Processor Definitions @@ -96,10 +98,70 @@ * Protected Functions ****************************************************************************/ +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + /**************************************************************************** * Public Functions ****************************************************************************/ +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + /************************************************************************************ * Name: stm32_boardinitialize * @@ -110,7 +172,8 @@ * ************************************************************************************/ -__EXPORT void stm32_boardinitialize(void) +__EXPORT void +stm32_boardinitialize(void) { /* configure SPI interfaces */ stm32_spiinitialize(); @@ -170,6 +233,9 @@ __EXPORT int nsh_archinitialize(void) /* configure the high-resolution time/callout interface */ hrt_init(); + /* configure the DMA allocator */ + dma_alloc_init(); + /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 818b64436..4d41d0d07 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -84,7 +84,7 @@ /* Power switch controls ******************************************************/ -#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 0ea95bded..ccd01edf5 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_BTN_SAFETY); - /* spektrum power enable is active high - disable it by default */ - /* XXX might not want to do this on warm restart? */ - stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + /* spektrum power enable is active high - enable it by default */ stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_SERVO_FAULT_DETECT); diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index dc0c84052..a9e22eaa6 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,15 +34,14 @@ /** * @file ringbuffer.h * - * A simple ringbuffer template. + * A flexible ringbuffer class. */ #pragma once -template<typename T> class RingBuffer { public: - RingBuffer(unsigned size); + RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); /** @@ -52,15 +50,37 @@ public: * @param val Item to put * @return true if the item was put, false if the buffer is full */ - bool put(T &val); + bool put(const void *val, size_t val_size = 0); + + bool put(int8_t val); + bool put(uint8_t val); + bool put(int16_t val); + bool put(uint16_t val); + bool put(int32_t val); + bool put(uint32_t val); + bool put(int64_t val); + bool put(uint64_t val); + bool put(float val); + bool put(double val); /** - * Put an item into the buffer. + * Force an item into the buffer, discarding an older item if there is not space. * * @param val Item to put - * @return true if the item was put, false if the buffer is full + * @return true if an item was discarded to make space */ - bool put(const T &val); + bool force(const void *val, size_t val_size = 0); + + bool force(int8_t val); + bool force(uint8_t val); + bool force(int16_t val); + bool force(uint16_t val); + bool force(int32_t val); + bool force(uint32_t val); + bool force(int64_t val); + bool force(uint64_t val); + bool force(float val); + bool force(double val); /** * Get an item from the buffer. @@ -68,15 +88,18 @@ public: * @param val Item that was gotten * @return true if an item was got, false if the buffer was empty. */ - bool get(T &val); + bool get(void *val, size_t val_size = 0); - /** - * Get an item from the buffer (scalars only). - * - * @return The value that was fetched, or zero if the buffer was - * empty. - */ - T get(void); + bool get(int8_t &val); + bool get(uint8_t &val); + bool get(int16_t &val); + bool get(uint16_t &val); + bool get(int32_t &val); + bool get(uint32_t &val); + bool get(int64_t &val); + bool get(uint64_t &val); + bool get(float &val); + bool get(double &val); /* * Get the number of slots free in the buffer. @@ -97,54 +120,103 @@ public: /* * Returns true if the buffer is empty. */ - bool empty() { return _tail == _head; } + bool empty(); /* * Returns true if the buffer is full. */ - bool full() { return _next(_head) == _tail; } + bool full(); /* * Returns the capacity of the buffer, or zero if the buffer could * not be allocated. */ - unsigned size() { return (_buf != nullptr) ? _size : 0; } + unsigned size(); /* * Empties the buffer. */ - void flush() { _head = _tail = _size; } + void flush(); + + /* + * resize the buffer. This is unsafe to be called while + * a producer or consuming is running. Caller is responsible + * for any locking needed + * + * @param new_size new size for buffer + * @return true if the resize succeeds, false if + * not (allocation error) + */ + bool resize(unsigned new_size); + + /* + * printf() some info on the buffer + */ + void print_info(const char *name); private: - T *const _buf; - const unsigned _size; - volatile unsigned _head; /**< insertion point */ - volatile unsigned _tail; /**< removal point */ + unsigned _num_items; + const size_t _item_size; + char *_buf; + volatile unsigned _head; /**< insertion point in _item_size units */ + volatile unsigned _tail; /**< removal point in _item_size units */ unsigned _next(unsigned index); }; -template <typename T> -RingBuffer<T>::RingBuffer(unsigned with_size) : - _buf(new T[with_size + 1]), - _size(with_size), - _head(with_size), - _tail(with_size) +RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : + _num_items(num_items), + _item_size(item_size), + _buf(new char[(_num_items+1) * item_size]), + _head(_num_items), + _tail(_num_items) {} -template <typename T> -RingBuffer<T>::~RingBuffer() +RingBuffer::~RingBuffer() { if (_buf != nullptr) delete[] _buf; } -template <typename T> -bool RingBuffer<T>::put(T &val) +unsigned +RingBuffer::_next(unsigned index) +{ + return (0 == index) ? _num_items : (index - 1); +} + +bool +RingBuffer::empty() +{ + return _tail == _head; +} + +bool +RingBuffer::full() +{ + return _next(_head) == _tail; +} + +unsigned +RingBuffer::size() +{ + return (_buf != nullptr) ? _num_items : 0; +} + +void +RingBuffer::flush() +{ + while (!empty()) + get(NULL); +} + +bool +RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); if (next != _tail) { - _buf[_head] = val; + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; } else { @@ -152,52 +224,286 @@ bool RingBuffer<T>::put(T &val) } } -template <typename T> -bool RingBuffer<T>::put(const T &val) +bool +RingBuffer::put(int8_t val) { - unsigned next = _next(_head); - if (next != _tail) { - _buf[_head] = val; - _head = next; - return true; - } else { - return false; + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint8_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint16_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint32_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(int64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(uint64_t val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(float val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::put(double val) +{ + return put(&val, sizeof(val)); +} + +bool +RingBuffer::force(const void *val, size_t val_size) +{ + bool overwrote = false; + + for (;;) { + if (put(val, val_size)) + break; + get(NULL); + overwrote = true; } + return overwrote; +} + +bool +RingBuffer::force(int8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint8_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint16_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(int32_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint32_t val) +{ + return force(&val, sizeof(val)); } -template <typename T> -bool RingBuffer<T>::get(T &val) +bool +RingBuffer::force(int64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(uint64_t val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(float val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::force(double val) +{ + return force(&val, sizeof(val)); +} + +bool +RingBuffer::get(void *val, size_t val_size) { if (_tail != _head) { - val = _buf[_tail]; - _tail = _next(_tail); + unsigned candidate; + unsigned next; + + if ((val_size == 0) || (val_size > _item_size)) + val_size = _item_size; + + do { + /* decide which element we think we're going to read */ + candidate = _tail; + + /* and what the corresponding next index will be */ + next = _next(candidate); + + /* go ahead and read from this index */ + if (val != NULL) + memcpy(val, &_buf[candidate * _item_size], val_size); + + /* if the tail pointer didn't change, we got our item */ + } while (!__sync_bool_compare_and_swap(&_tail, candidate, next)); + return true; } else { return false; } } -template <typename T> -T RingBuffer<T>::get(void) +bool +RingBuffer::get(int8_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint8_t &val) { - T val; - return get(val) ? val : 0; + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::space(void) +bool +RingBuffer::get(int16_t &val) { - return (_tail >= _head) ? (_size - (_tail - _head)) : (_head - _tail - 1); + return get(&val, sizeof(val)); } -template <typename T> -unsigned RingBuffer<T>::count(void) +bool +RingBuffer::get(uint16_t &val) { - return _size - space(); + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint32_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(int64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(uint64_t &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(float &val) +{ + return get(&val, sizeof(val)); +} + +bool +RingBuffer::get(double &val) +{ + return get(&val, sizeof(val)); +} + +unsigned +RingBuffer::space(void) +{ + unsigned tail, head; + + /* + * Make a copy of the head/tail pointers in a fashion that + * may err on the side of under-estimating the free space + * in the buffer in the case that the buffer is being updated + * asynchronously with our check. + * If the head pointer changes (reducing space) while copying, + * re-try the copy. + */ + do { + head = _head; + tail = _tail; + } while (head != _head); + + return (tail >= head) ? (_num_items - (tail - head)) : (head - tail - 1); +} + +unsigned +RingBuffer::count(void) +{ + /* + * Note that due to the conservative nature of space(), this may + * over-estimate the number of items in the buffer. + */ + return _num_items - space(); +} + +bool +RingBuffer::resize(unsigned new_size) +{ + char *old_buffer; + char *new_buffer = new char [(new_size+1) * _item_size]; + if (new_buffer == nullptr) { + return false; + } + old_buffer = _buf; + _buf = new_buffer; + _num_items = new_size; + _head = new_size; + _tail = new_size; + delete[] old_buffer; + return true; } -template <typename T> -unsigned RingBuffer<T>::_next(unsigned index) +void +RingBuffer::print_info(const char *name) { - return (0 == index) ? _size : (index - 1); + printf("%s %u/%u (%u/%u @ %p)\n", + name, + _num_items, + _num_items * _item_size, + _head, + _tail, + _buf); } diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 8fffd60cb..fa6b78d64 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -67,6 +67,7 @@ SPI::SPI(const char *name, CDev(name, devname, irq), // public // protected + locking_mode(LOCK_PREEMPTION), // private _bus(bus), _device(device), @@ -132,13 +133,25 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + irqstate_t state; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; - /* do common setup */ - if (!up_interrupt_context()) - SPI_LOCK(_dev, true); + /* lock the bus as required */ + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + state = irqsave(); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + break; + case LOCK_NONE: + break; + } + } SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); @@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) - SPI_LOCK(_dev, false); + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + irqrestore(state); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + break; + } + } return OK; } diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index e0122372a..9103dca2e 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + LockMode locking_mode; /**< selected locking mode */ + private: int _bus; enum spi_dev_e _device; diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 794de584b..7d93ed938 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Accelerometer driver interface. + * @file drv_accel.h + * + * Accelerometer driver interface. */ #ifndef _DRV_ACCEL_H @@ -52,6 +54,7 @@ */ struct accel_report { uint64_t timestamp; + uint64_t error_count; float x; /**< acceleration in the NED X board axis in m/s^2 */ float y; /**< acceleration in the NED Y board axis in m/s^2 */ float z; /**< acceleration in the NED Z board axis in m/s^2 */ @@ -65,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; diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h index 7bb9ee2af..78f31495d 100644 --- a/src/drivers/drv_airspeed.h +++ b/src/drivers/drv_airspeed.h @@ -32,7 +32,10 @@ ****************************************************************************/ /** - * @file Airspeed driver interface. + * @file drv_airspeed.h + * + * Airspeed driver interface. + * * @author Simon Wilks */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 176f798c0..e2f0137ae 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Barometric pressure sensor driver interface. + * @file drv_baro.h + * + * Barometric pressure sensor driver interface. */ #ifndef _DRV_BARO_H @@ -55,6 +57,7 @@ struct baro_report { float altitude; float temperature; uint64_t timestamp; + uint64_t error_count; }; /* diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 1dda8ef0b..06e3535b3 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file GPS driver interface. + * @file drv_gps.h + * + * GPS driver interface. */ #ifndef _DRV_GPS_H @@ -51,8 +53,7 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_NMEA, + GPS_DRIVER_MODE_MTK } gps_driver_mode_t; diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 1d0fef6fc..2ae8c7058 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -32,7 +32,9 @@ ****************************************************************************/ /** - * @file Gyroscope driver interface. + * @file drv_gyro.h + * + * Gyroscope driver interface. */ #ifndef _DRV_GYRO_H @@ -52,6 +54,7 @@ */ struct gyro_report { uint64_t timestamp; + uint64_t error_count; float x; /**< angular velocity in the NED X board axis in rad/s */ float y; /**< angular velocity in the NED Y board axis in rad/s */ float z; /**< angular velocity in the NED Z board axis in rad/s */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3de5625fd..06107bd3d 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -54,6 +54,7 @@ */ struct mag_report { uint64_t timestamp; + uint64_t error_count; float x; float y; float z; diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6ed9320cb..efd6afb4b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,6 +65,26 @@ __BEGIN_DECLS #define PWM_OUTPUT_MAX_CHANNELS 16 /** + * Minimum PWM in us + */ +#define PWM_MIN 900 + +/** + * Highest PWM allowed as the minimum PWM + */ +#define PWM_HIGHEST_MIN 1300 + +/** + * Maximum PWM in us + */ +#define PWM_MAX 2100 + +/** + * Lowest PWM allowed as the maximum PWM + */ +#define PWM_LOWEST_MAX 1700 + +/** * Servo output signal type, value is actual servo output pulse * width in microseconds. */ @@ -79,7 +99,7 @@ typedef uint16_t servo_position_t; struct pwm_output_values { /** desired pulse widths for each of the supported channels */ servo_position_t values[PWM_OUTPUT_MAX_CHANNELS]; - int channel_count; + unsigned channel_count; }; /* @@ -101,35 +121,63 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) +/** get default servo update rate */ +#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) + /** set alternate servo update rate */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3) + +/** get alternate servo update rate */ +#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** get the number of servos in *(unsigned *)arg */ -#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) +#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5) /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ -#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) +#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6) + +/** check the selected update rates */ +#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7) /** set the 'ARM ok' bit, which activates the safety switch */ -#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5) +#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8) /** clear the 'ARM ok' bit, which deactivates the safety switch */ -#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9) /** start DSM bind */ -#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10) + +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11) + +/** set the PWM value for failsafe */ +#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12) + +/** get the PWM value for failsafe */ +#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13) /** set the PWM value when disarmed - should be no PWM (zero) by default */ -#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 9) +#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14) + +/** get the PWM value when disarmed */ +#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15) /** set the minimum PWM value the output will send */ -#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 10) +#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16) + +/** get the minimum PWM value the output will send */ +#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17) /** set the maximum PWM value the output will send */ -#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 11) +#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18) + +/** get the maximum PWM value the output will send */ +#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 03a82ec5d..cf91f7030 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -52,6 +52,7 @@ */ struct range_finder_report { uint64_t timestamp; + uint64_t error_count; float distance; /** in meters */ uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ }; diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 4decc324e..78ffad9d7 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -32,8 +32,9 @@ ****************************************************************************/ /** - * @file R/C input interface. + * @file drv_rc_input.h * + * R/C input interface. */ #ifndef _DRV_RC_INPUT_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 3a89cab9d..8e8b2c1b9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index f0b860620..cb5fd53a5 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -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" @@ -60,6 +62,8 @@ #include <sys/ioctl.h> +#define TONEALARM_DEVICE_PATH "/dev/tone_alarm" + #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) @@ -142,6 +146,7 @@ enum { TONE_ARMING_WARNING_TUNE, TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, + TONE_GPS_WARNING_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 257b41935..9d8ad084e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -68,6 +68,7 @@ #include <drivers/drv_airspeed.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/differential_pressure.h> @@ -131,11 +132,8 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } @@ -152,48 +150,44 @@ ETSAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + perf_count(_comms_errors); return ret; } uint16_t diff_pres_pa = val[1] << 8 | val[0]; if (diff_pres_pa == 0) { - // a zero value means the pressure sensor cannot give us a - // value. We need to return, and not report a value or the - // caller could end up using this value as part of an - // average - log("zero value from sensor"); - return -1; + // a zero value means the pressure sensor cannot give us a + // value. We need to return, and not report a value or the + // caller could end up using this value as part of an + // average + perf_count(_comms_errors); + log("zero value from sensor"); + return -1; } if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; - } else { diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].differential_pressure_pa = diff_pres_pa; - // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + // XXX we may want to smooth out the readings to remove noise. + differential_pressure_s report; + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.differential_pressure_pa = diff_pres_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -213,7 +207,7 @@ ETSAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + perf_count(_comms_errors); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 38835418b..fc500a9ec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char* uart_path); + GPS(const char *uart_path); virtual ~GPS(); virtual int init(); @@ -156,7 +156,7 @@ GPS *g_dev; } -GPS::GPS(const char* uart_path) : +GPS::GPS(const char *uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -192,6 +192,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); + g_dev = nullptr; } @@ -270,19 +271,20 @@ GPS::task_main() } switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; } + unlock(); + if (_Helper->configure(_baudrate) == 0) { unlock(); @@ -294,6 +296,7 @@ GPS::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } @@ -310,10 +313,26 @@ GPS::task_main() } if (!_healthy) { - warnx("module found"); + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); _healthy = true; } } + if (_healthy) { warnx("module lost"); _healthy = false; @@ -322,25 +341,26 @@ GPS::task_main() lock(); } + lock(); /* select next mode */ switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + + default: + break; } } - debug("exiting"); + + warnx("exiting"); ::close(_serial_fd); @@ -361,23 +381,25 @@ void GPS::print_info() { switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; + + default: + break; } + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + if (_report.timestamp_position != 0) { - warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, + _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); @@ -428,6 +450,7 @@ start(const char *uart_path) errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } + exit(0); fail: @@ -503,7 +526,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = GPS_DEFAULT_UART_PORT; + char *device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. @@ -513,15 +536,18 @@ gps_main(int argc, char *argv[]) if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; + } else { goto out; } } + gps::start(device_name); } if (!strcmp(argv[1], "stop")) gps::stop(); + /* * Test the driver/device. */ diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index ba86d370a..2e2cbc8dd 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); + warnx("try baudrate: %d\n", speed); default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } + struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); return -1; } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index defc1a074..73d4b889c 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -33,7 +33,7 @@ * ****************************************************************************/ -/** +/** * @file gps_helper.h */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 62941d74b..56b702ea6 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -48,9 +48,9 @@ MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_mtk_revision(0) + _fd(fd), + _gps_position(gps_position), + _mtk_revision(0) { decode_init(); } @@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate) warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { @@ -128,12 +132,15 @@ MTK::receive(unsigned timeout) handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } + j++; } + /* everything is read */ j = count = 0; } @@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 19; @@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) add_byte_to_checksum(b); // Fill packet buffer - ((uint8_t*)(&packet))[_rx_count] = b; + ((uint8_t *)(&packet))[_rx_count] = b; _rx_count++; /* Packet size minus checksum, XXX ? */ @@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) /* Compare checksum */ if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { ret = 1; + } else { warnx("MTK Checksum invalid"); ret = -1; } + // Reset state machine to decode next packet decode_init(); } } + return ret; } @@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet) if (_mtk_revision == 16) { _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { _gps_position->lat = packet.latitude; // both degrees*1e7 _gps_position->lon = packet.longitude; // both degrees*1e7 + } else { warnx("mtk: unknown revision"); _gps_position->lat = 0; _gps_position->lon = 0; } + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit _gps_position->epv_m = 0.0; //unknown in mtk custom mode - _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ba5d14cc4..86291901c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -60,13 +60,14 @@ #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), _configured(false), _waiting_for_ack(false), - _disable_cmd_counter(0) + _disable_cmd_last(0) { decode_init(); } @@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate) configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + warnx("ubx: msg rate configuration failed: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SOL\n"); + warnx("ubx: msg rate configuration failed: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + warnx("ubx: msg rate configuration failed: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + warnx("ubx: msg rate configuration failed: NAV SVINFO"); return 1; } @@ -271,11 +272,17 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ + warnx("ubx: poll error"); return -1; } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - return handled ? 1 : -1; + if (handled) { + return 1; + + } else { + return -1; + } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -292,8 +299,6 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ if (handle_message() > 0) handled = true; } @@ -303,6 +308,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { + warnx("ubx: timeout - no useful messages"); return -1; } } @@ -453,16 +459,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME,&ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME, &ts); #endif - + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); @@ -564,10 +570,13 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + + hrt_abstime t = hrt_absolute_time(); - if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } @@ -640,7 +649,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length) ck_b = ck_b + ck_a; } - /* The checksum is written to the last to bytes of a message */ + /* the checksum is written to the last to bytes of a message */ message[length - 2] = ck_a; message[length - 1] = ck_b; } @@ -669,17 +678,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; - /* Calculate the checksum now */ + /* calculate the checksum now */ add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; - /* Start with the two sync bytes */ + /* start with the two sync bytes */ ret += write(fd, sync_bytes, sizeof(sync_bytes)); ret += write(fd, packet, length); if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: config write fail"); + warnx("ubx: configuration write fail"); } void @@ -696,7 +705,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); - // Configure receive check + /* configure ACK check */ _message_class_needed = msg_class; _message_id_needed = msg_id; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 4fc276975..76ef873a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -347,7 +347,7 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t *message, const unsigned length); /** * Helper to send a config packet @@ -358,7 +358,7 @@ private: void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); int wait_for_ack(unsigned timeout); @@ -376,7 +376,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_counter; + uint8_t _disable_cmd_last; }; #endif /* UBX_H_ */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 3e30e3292..c1d73dd87 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -404,7 +404,7 @@ HIL::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (i < (unsigned)outputs.noutputs && + if (i < outputs.noutputs && isfinite(outputs.output[i]) && outputs.output[i] >= -1.0f && outputs.output[i] <= 1.0f) { @@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) g_hil->set_pwm_rate(arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: // HIL always outputs at the alternate (usually faster) rate break; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 3ede90a17..5f0ce4ff8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -65,6 +65,7 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -148,10 +149,7 @@ private: work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - mag_report *_reports; + RingBuffer *_reports; mag_scale _scale; float _range_scale; float _range_ga; @@ -310,9 +308,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -322,9 +317,6 @@ extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); HMC5883::HMC5883(int bus) : I2C("HMC5883", MAG_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), @@ -356,9 +348,8 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -375,21 +366,18 @@ HMC5883::init() if (I2C::init() != OK) goto out; - /* reset the device configuration */ - reset(); - /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct mag_report[_num_reports]; - + _reports = new RingBuffer(2, sizeof(mag_report)); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; + /* reset the device configuration */ + reset(); /* get a publish handle on the mag topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_reports[0]); + struct mag_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report); if (_mag_topic < 0) debug("failed to create sensor_mag object"); @@ -493,6 +481,7 @@ ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -501,17 +490,15 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_measure_ticks > 0) { - /* * While there is space in the caller's buffer, and reports, copy them. * Note that we may be pre-empted by the workq thread while we are doing this; * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; } } @@ -522,7 +509,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) /* manual measurement - run one conversion */ /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -539,10 +526,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) break; } - /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); - + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } } while (0); return ret; @@ -615,31 +601,22 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000/TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: return reset(); @@ -701,7 +678,7 @@ HMC5883::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&HMC5883::cycle_trampoline, this, 1); @@ -810,9 +787,11 @@ HMC5883::collect() perf_begin(_sample_perf); + struct mag_report new_report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that @@ -842,8 +821,10 @@ HMC5883::collect() */ if ((abs(report.x) > 2048) || (abs(report.y) > 2048) || - (abs(report.z) > 2048)) + (abs(report.z) > 2048)) { + perf_count(_comms_errors); goto out; + } /* * RAW outputs @@ -851,10 +832,10 @@ HMC5883::collect() * to align the sensor axes with the board, x and y need to be flipped * and y needs to be negated */ - _reports[_next_report].x_raw = report.y; - _reports[_next_report].y_raw = ((report.x == -32768) ? 32767 : -report.x); + new_report.x_raw = report.y; + new_report.y_raw = -report.x; /* z remains z */ - _reports[_next_report].z_raw = report.z; + new_report.z_raw = report.z; /* scale values for output */ @@ -876,34 +857,30 @@ HMC5883::collect() #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { /* to align the sensor axes with the board, x and y need to be flipped */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, * therefore switch x and y and invert y */ - _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); - - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + /* post a report to the ring */ + if (_reports->force(&new_report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -1222,8 +1199,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** @@ -1332,7 +1308,7 @@ test() errx(1, "failed to get if mag is onboard or external"); warnx("device active: %s", ret ? "external" : "onboard"); - /* set the queue depth to 10 */ + /* set the queue depth to 5 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e6d765e13..8f5674823 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -61,6 +61,7 @@ #include <drivers/drv_hrt.h> #include <drivers/device/spi.h> #include <drivers/drv_gyro.h> +#include <drivers/device/ringbuffer.h> #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> @@ -183,11 +184,8 @@ private: struct hrt_call _call; unsigned _call_interval; - - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct gyro_report *_reports; + + RingBuffer *_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -299,16 +297,9 @@ private: int self_test(); }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -340,7 +331,7 @@ L3GD20::~L3GD20() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; /* delete the perf counter */ perf_free(_sample_perf); @@ -356,16 +347,15 @@ L3GD20::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _oldest_report = _next_report = 0; - _reports = new struct gyro_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(gyro_report)); if (_reports == nullptr) goto out; /* advertise sensor topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_reports[0]); + struct gyro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report); reset(); @@ -380,6 +370,8 @@ L3GD20::probe() /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); + bool success = false; + /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { @@ -390,15 +382,19 @@ L3GD20::probe() #else #error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 #endif - return OK; + + success = true; } if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; - return OK; + success = true; } + if (success) + return OK; + return -EIO; } @@ -406,6 +402,7 @@ ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct gyro_report); + struct gyro_report *gbuf = reinterpret_cast<struct gyro_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +418,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(gbuf)) { + ret += sizeof(*gbuf); + gbuf++; } } @@ -433,12 +429,13 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_report = _next_report = 0; + _reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(gbuf)) { + ret = sizeof(*gbuf); + } return ret; } @@ -506,31 +503,22 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct gyro_report *buf = new struct gyro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: reset(); @@ -699,7 +687,7 @@ L3GD20::start() stop(); /* reset the report ring */ - _oldest_report = _next_report = 0; + _reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&L3GD20::measure_trampoline, this); @@ -759,7 +747,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report *report = &_reports[_next_report]; + gyro_report report; /* start the performance counter */ perf_begin(_sample_perf); @@ -782,61 +770,57 @@ L3GD20::measure() * the offset is 74 from the origin and subtracting * 74 from all measurements centers them around zero. */ - report->timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = 0; // not recorded switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: /* keep axes in place */ - report->x_raw = raw_report.x; - report->y_raw = raw_report.y; + report.x_raw = raw_report.x; + report.y_raw = raw_report.y; break; case SENSOR_BOARD_ROTATION_090_DEG: /* swap x and y */ - report->x_raw = raw_report.y; - report->y_raw = raw_report.x; + report.x_raw = raw_report.y; + report.y_raw = raw_report.x; break; case SENSOR_BOARD_ROTATION_180_DEG: /* swap x and y and negate both */ - report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); - report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); + report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); break; case SENSOR_BOARD_ROTATION_270_DEG: /* swap x and y and negate y */ - report->x_raw = raw_report.y; - report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); + report.x_raw = raw_report.y; + report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x); break; } - report->z_raw = raw_report.z; - - report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; - report->y = ((report->y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; - report->z = ((report->z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + report.z_raw = raw_report.z; - report->x = _gyro_filter_x.apply(report->x); - report->y = _gyro_filter_y.apply(report->y); - report->z = _gyro_filter_z.apply(report->z); + report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - report->scaling = _gyro_range_scale; - report->range_rad_s = _gyro_range_rad_s; + report.x = _gyro_filter_x.apply(report.x); + report.y = _gyro_filter_y.apply(report.y); + report.z = _gyro_filter_z.apply(report.z); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + report.scaling = _gyro_range_scale; + report.range_rad_s = _gyro_range_rad_s; - /* if we are running up against the oldest report, fix it */ - if (_next_report == _oldest_report) - INCREMENT(_oldest_report, _num_reports); + _reports->force(&report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ if (_gyro_topic > 0) - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); _read++; @@ -849,8 +833,7 @@ L3GD20::print_info() { printf("gyro reads: %u\n", _read); perf_print_counter(_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } int diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 05d6f1881..60601e22c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -62,6 +62,7 @@ #include <drivers/device/spi.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/device/ringbuffer.h> #include <board_config.h> #include <mathlib/math/filter/LowPassFilter2p.hpp> @@ -218,15 +219,8 @@ private: unsigned _call_accel_interval; unsigned _call_mag_interval; - unsigned _num_accel_reports; - volatile unsigned _next_accel_report; - volatile unsigned _oldest_accel_report; - struct accel_report *_accel_reports; - - unsigned _num_mag_reports; - volatile unsigned _next_mag_report; - volatile unsigned _oldest_mag_report; - struct mag_report *_mag_reports; + RingBuffer *_accel_reports; + RingBuffer *_mag_reports; struct accel_scale _accel_scale; unsigned _accel_range_m_s2; @@ -247,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. */ @@ -404,7 +405,7 @@ public: LSM303D_mag(LSM303D *parent); ~LSM303D_mag(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); protected: @@ -420,22 +421,12 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - - LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), - _num_accel_reports(0), - _next_accel_report(0), - _oldest_accel_report(0), _accel_reports(nullptr), - _num_mag_reports(0), - _next_mag_report(0), - _oldest_mag_report(0), _mag_reports(nullptr), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), @@ -450,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; @@ -480,9 +475,9 @@ LSM303D::~LSM303D() /* free any existing reports */ if (_accel_reports != nullptr) - delete[] _accel_reports; + delete _accel_reports; if (_mag_reports != nullptr) - delete[] _mag_reports; + delete _mag_reports; delete _mag; @@ -498,24 +493,23 @@ LSM303D::init() int mag_ret; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { + warnx("SPI init failed"); goto out; + } /* allocate basic report buffers */ - _num_accel_reports = 2; - _oldest_accel_report = _next_accel_report = 0; - _accel_reports = new struct accel_report[_num_accel_reports]; + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; /* advertise accel topic */ - memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); + struct accel_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report); - _num_mag_reports = 2; - _oldest_mag_report = _next_mag_report = 0; - _mag_reports = new struct mag_report[_num_mag_reports]; + _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) goto out; @@ -523,8 +517,9 @@ LSM303D::init() reset(); /* advertise mag topic */ - memset(&_mag_reports[0], 0, sizeof(_mag_reports[0])); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]); + struct mag_report zero_mag_report; + memset(&zero_mag_report, 0, sizeof(zero_mag_report)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_mag_report); /* do CDev init for the mag device node, keep it optional */ mag_ret = _mag->init(); @@ -542,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); @@ -567,7 +564,9 @@ LSM303D::probe() (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) + bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); + + if (success) return OK; return -EIO; @@ -577,6 +576,7 @@ ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct accel_report); + accel_report *arb = reinterpret_cast<accel_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -585,17 +585,13 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { - /* * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_accel_report != _next_accel_report) { - memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); - ret += sizeof(_accel_reports[0]); - INCREMENT(_oldest_accel_report, _num_accel_reports); + if (_accel_reports->get(arb)) { + ret += sizeof(*arb); + arb++; } } @@ -604,12 +600,11 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_accel_report = _next_accel_report = 0; measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); - ret = sizeof(*_accel_reports); + if (_accel_reports->get(arb)) + ret = sizeof(*arb); return ret; } @@ -618,6 +613,7 @@ ssize_t LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct mag_report); + mag_report *mrb = reinterpret_cast<mag_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -629,14 +625,11 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the measurement code while we are doing this; - * we are careful to avoid racing with it. */ while (count--) { - if (_oldest_mag_report != _next_mag_report) { - memcpy(buffer, _mag_reports + _oldest_mag_report, sizeof(*_mag_reports)); - ret += sizeof(_mag_reports[0]); - INCREMENT(_oldest_mag_report, _num_mag_reports); + if (_mag_reports->get(mrb)) { + ret += sizeof(*mrb); + mrb++; } } @@ -645,12 +638,12 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement */ - _oldest_mag_report = _next_mag_report = 0; + _mag_reports->flush(); measure(); /* measurement will have generated a report, copy it out */ - memcpy(buffer, _mag_reports, sizeof(*_mag_reports)); - ret = sizeof(*_mag_reports); + if (_mag_reports->get(mrb)) + ret = sizeof(*mrb); return ret; } @@ -718,31 +711,22 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) + if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - struct accel_report *buf = new struct accel_report[arg]; - - if (nullptr == buf) + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _accel_reports; - _num_accel_reports = arg; - _accel_reports = buf; - start(); + } + irqrestore(flags); return OK; } case SENSORIOCGQUEUEDEPTH: - return _num_accel_reports - 1; + return _accel_reports->size(); case SENSORIOCRESET: reset(); @@ -854,31 +838,22 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* account for sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct mag_report *buf = new struct mag_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _mag_reports; - _num_mag_reports = arg; - _mag_reports = buf; - start(); + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - return OK; + irqstate_t flags = irqsave(); + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_mag_reports - 1; + return _mag_reports->size(); case SENSORIOCRESET: reset(); @@ -1046,6 +1021,7 @@ LSM303D::accel_set_range(unsigned max_g) _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; @@ -1170,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency) } modify_reg(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; return OK; } @@ -1211,8 +1188,8 @@ LSM303D::start() stop(); /* reset the report ring */ - _oldest_accel_report = _next_accel_report = 0; - _oldest_mag_report = _next_mag_report = 0; + _accel_reports->flush(); + _mag_reports->flush(); /* start polling at the specified rate */ hrt_call_every(&_accel_call, 1000, _call_accel_interval, (hrt_callout)&LSM303D::measure_trampoline, this); @@ -1247,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) @@ -1259,7 +1242,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - accel_report *accel_report = &_accel_reports[_next_accel_report]; + accel_report accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1284,35 +1267,31 @@ LSM303D::measure() */ - accel_report->timestamp = hrt_absolute_time(); - - accel_report->x_raw = raw_accel_report.x; - accel_report->y_raw = raw_accel_report.y; - accel_report->z_raw = raw_accel_report.z; + accel_report.timestamp = hrt_absolute_time(); + accel_report.error_count = 0; // not reported - float x_in_new = ((accel_report->x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - float y_in_new = ((accel_report->y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - float z_in_new = ((accel_report->z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + accel_report.x_raw = raw_accel_report.x; + accel_report.y_raw = raw_accel_report.y; + accel_report.z_raw = raw_accel_report.z; - accel_report->x = _accel_filter_x.apply(x_in_new); - accel_report->y = _accel_filter_y.apply(y_in_new); - accel_report->z = _accel_filter_z.apply(z_in_new); + float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - accel_report->scaling = _accel_range_scale; - accel_report->range_m_s2 = _accel_range_m_s2; + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_accel_report, _num_accel_reports); + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; - /* if we are running up against the oldest report, fix it */ - if (_next_accel_report == _oldest_accel_report) - INCREMENT(_oldest_accel_report, _num_accel_reports); + _accel_reports->force(&accel_report); /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); _accel_read++; @@ -1323,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 { @@ -1334,7 +1319,7 @@ LSM303D::mag_measure() } raw_mag_report; #pragma pack(pop) - mag_report *mag_report = &_mag_reports[_next_mag_report]; + mag_report mag_report; /* start the performance counter */ perf_begin(_mag_sample_perf); @@ -1359,30 +1344,25 @@ LSM303D::mag_measure() */ - mag_report->timestamp = hrt_absolute_time(); - - mag_report->x_raw = raw_mag_report.x; - mag_report->y_raw = raw_mag_report.y; - mag_report->z_raw = raw_mag_report.z; - mag_report->x = ((mag_report->x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report->y = ((mag_report->y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report->z = ((mag_report->z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report->scaling = _mag_range_scale; - mag_report->range_ga = (float)_mag_range_ga; + mag_report.timestamp = hrt_absolute_time(); - /* post a report to the ring - note, not locked */ - INCREMENT(_next_mag_report, _num_mag_reports); + mag_report.x_raw = raw_mag_report.x; + mag_report.y_raw = raw_mag_report.y; + mag_report.z_raw = raw_mag_report.z; + mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; - /* if we are running up against the oldest report, fix it */ - if (_next_mag_report == _oldest_mag_report) - INCREMENT(_oldest_mag_report, _num_mag_reports); + _mag_reports->force(&mag_report); /* XXX please check this poll_notify, is it the right one? */ /* notify anyone waiting for data */ poll_notify(POLLIN); /* publish for subscribers */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, mag_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); _mag_read++; @@ -1396,11 +1376,8 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_accel_reports, _oldest_accel_report, _next_accel_report, _accel_reports); - perf_print_counter(_mag_sample_perf); - printf("report queue: %u (%u/%u @ %p)\n", - _num_mag_reports, _oldest_mag_report, _next_mag_report, _mag_reports); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); } LSM303D_mag::LSM303D_mag(LSM303D *parent) : @@ -1470,8 +1447,10 @@ start() /* create the driver */ g_dev = new LSM303D(1 /* XXX magic number */, ACCEL_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); - if (g_dev == nullptr) + if (g_dev == nullptr) { + warnx("failed instantiating LSM303D obj"); goto fail; + } if (OK != g_dev->init()) goto fail; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f83416993..c910e2890 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -64,6 +64,7 @@ #include <drivers/drv_hrt.h> #include <drivers/drv_range_finder.h> +#include <drivers/device/ringbuffer.h> #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> @@ -119,10 +120,7 @@ private: float _min_distance; float _max_distance; work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - range_finder_report *_reports; + RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; @@ -183,9 +181,6 @@ private: }; -/* helper macro for handling report buffer indices */ -#define INCREMENT(_x, _lim) do { __typeof__(_x) _tmp = _x+1; if (_tmp >= _lim) _tmp = 0; _x = _tmp; } while(0) - /* * Driver 'main' command. */ @@ -195,9 +190,6 @@ MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -221,7 +213,7 @@ MB12XX::~MB12XX() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; } int @@ -234,17 +226,15 @@ MB12XX::init() goto out; /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct range_finder_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) goto out; - _oldest_report = _next_report = 0; - /* get a publish handle on the range finder topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) debug("failed to create sensor_range_finder object. Did you start uOrb?"); @@ -354,31 +344,22 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct range_finder_report *buf = new struct range_finder_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -406,6 +387,7 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -421,10 +403,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; } } @@ -433,9 +414,8 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { - _oldest_report = _next_report = 0; + _reports->flush(); /* trigger a measurement */ if (OK != measure()) { @@ -453,8 +433,9 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } } while (0); @@ -498,26 +479,26 @@ MB12XX::collect() if (ret < 0) { log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + struct range_finder_report report; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].distance = si_units; - _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; /* publish it */ - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -525,11 +506,8 @@ MB12XX::collect() ret = OK; -out: perf_end(_sample_perf); return ret; - - return ret; } void @@ -537,7 +515,7 @@ MB12XX::start() { /* reset the report ring and state machine */ _collect_phase = false; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); @@ -626,8 +604,7 @@ MB12XX::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b1cb2b3d8..b3003fc1b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,12 +138,8 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - return ret; } - ret = OK; - return ret; } @@ -161,7 +157,6 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -199,27 +194,24 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); - - _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temperature; - _reports[_next_report].differential_pressure_pa = diff_press_pa; + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa; } - /* announce the airspeed if needed, just publish else */ - orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &_reports[_next_report]); + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.temperature = temperature; + report.differential_pressure_pa = diff_press_pa; + report.voltage = 0; + report.max_differential_pressure_pa = _max_differential_pressure_pa; - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + /* announce the airspeed if needed, just publish else */ + orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { - perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); - } + new_report(report); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -239,7 +231,6 @@ MEASAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1bc3e97a4..b93f38cf6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -96,9 +96,10 @@ class MK : public device::I2C { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; enum MappingMode { @@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + /* switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) debug("not in a PWM mode"); break; } + */ + ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) @@ -1066,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) ret = OK; break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = OK; break; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14f8f44b8..70359110e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -194,16 +194,14 @@ private: struct hrt_call _call; unsigned _call_interval; - typedef RingBuffer<accel_report> AccelReportBuffer; - AccelReportBuffer *_accel_reports; + RingBuffer *_accel_reports; struct accel_scale _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - typedef RingBuffer<gyro_report> GyroReportBuffer; - GyroReportBuffer *_gyro_reports; + RingBuffer *_gyro_reports; struct gyro_scale _gyro_scale; float _gyro_range_scale; @@ -431,11 +429,11 @@ MPU6000::init() } /* allocate basic report buffers */ - _accel_reports = new AccelReportBuffer(2); + _accel_reports = new RingBuffer(2, sizeof(accel_report)); if (_accel_reports == nullptr) goto out; - _gyro_reports = new GyroReportBuffer(2); + _gyro_reports = new RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) goto out; @@ -466,14 +464,14 @@ MPU6000::init() _gyro_topic = -1; } else { gyro_report gr; - _gyro_reports->get(gr); + _gyro_reports->get(&gr); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); } /* advertise accel topic */ accel_report ar; - _accel_reports->get(ar); + _accel_reports->get(&ar); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); out: @@ -658,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) accel_report *arp = reinterpret_cast<accel_report *>(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(*arp++)) + if (!_accel_reports->get(arp)) break; transferred++; + arp++; } /* return the number of bytes transferred */ @@ -748,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) return -EAGAIN; /* copy reports out of our buffer to the caller */ - gyro_report *arp = reinterpret_cast<gyro_report *>(buffer); + gyro_report *grp = reinterpret_cast<gyro_report *>(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(*arp++)) + if (!_gyro_reports->get(grp)) break; transferred++; + grp++; } /* return the number of bytes transferred */ @@ -837,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - AccelReportBuffer *buf = new AccelReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; - return -ENOMEM; - } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _accel_reports; - _accel_reports = buf; - start(); - - return OK; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; + + irqstate_t flags = irqsave(); + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -935,21 +926,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) if ((arg < 1) || (arg > 100)) return -EINVAL; - /* allocate new buffer */ - GyroReportBuffer *buf = new GyroReportBuffer(arg); - - if (nullptr == buf) - return -ENOMEM; - if (buf->size() == 0) { - delete buf; + irqstate_t flags = irqsave(); + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); return -ENOMEM; } - - /* reset the measurement state machine with the new buffer, free the old */ - stop(); - delete _gyro_reports; - _gyro_reports = buf; - start(); + irqrestore(flags); return OK; } @@ -1204,7 +1186,7 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - + grb.error_count = arb.error_count = 0; // not reported /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1260,8 +1242,8 @@ MPU6000::measure() grb.temperature_raw = report.temp; grb.temperature = (report.temp) / 361.0f + 35.0f; - _accel_reports->put(arb); - _gyro_reports->put(grb); + _accel_reports->force(&arb); + _gyro_reports->force(&grb); /* notify anyone waiting for data */ poll_notify(POLLIN); @@ -1280,7 +1262,10 @@ MPU6000::measure() void MPU6000::print_info() { + perf_print_counter(_sample_perf); printf("reads: %u\n", _reads); + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); } MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 4e43f19c5..938786d3f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -60,6 +60,7 @@ #include <drivers/device/device.h> #include <drivers/drv_baro.h> #include <drivers/drv_hrt.h> +#include <drivers/device/ringbuffer.h> #include <systemlib/perf_counter.h> #include <systemlib/err.h> @@ -114,10 +115,7 @@ protected: struct work_s _work; unsigned _measure_ticks; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - struct baro_report *_reports; + RingBuffer *_reports; bool _collect_phase; unsigned _measure_phase; @@ -196,9 +194,6 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : _interface(interface), _prom(prom_buf.s), _measure_ticks(0), - _num_reports(0), - _next_report(0), - _oldest_report(0), _reports(nullptr), _collect_phase(false), _measure_phase(0), @@ -223,7 +218,7 @@ MS5611::~MS5611() /* free any existing reports */ if (_reports != nullptr) - delete[] _reports; + delete _reports; // free perf counters perf_free(_sample_perf); @@ -246,8 +241,7 @@ MS5611::init() } /* allocate basic report buffers */ - _num_reports = 2; - _reports = new struct baro_report[_num_reports]; + _reports = new RingBuffer(2, sizeof(baro_report)); if (_reports == nullptr) { debug("can't get memory for reports"); @@ -255,11 +249,10 @@ MS5611::init() goto out; } - _oldest_report = _next_report = 0; - /* get a publish handle on the baro topic */ - memset(&_reports[0], 0, sizeof(_reports[0])); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &_reports[0]); + struct baro_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report); if (_baro_topic < 0) { debug("failed to create sensor_baro object"); @@ -276,6 +269,7 @@ ssize_t MS5611::read(struct file *filp, char *buffer, size_t buflen) { unsigned count = buflen / sizeof(struct baro_report); + struct baro_report *brp = reinterpret_cast<struct baro_report *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -291,10 +285,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) * we are careful to avoid racing with them. */ while (count--) { - if (_oldest_report != _next_report) { - memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); - ret += sizeof(_reports[0]); - INCREMENT(_oldest_report, _num_reports); + if (_reports->get(brp)) { + ret += sizeof(*brp); + brp++; } } @@ -303,10 +296,9 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ do { _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* do temperature first */ if (OK != measure()) { @@ -335,8 +327,8 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - memcpy(buffer, _reports, sizeof(*_reports)); - ret = sizeof(*_reports); + if (_reports->get(brp)) + ret = sizeof(*brp); } while (0); @@ -411,31 +403,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* add one to account for the sentinel in the ring */ - arg++; - - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 2) || (arg > 100)) - return -EINVAL; - - /* allocate new buffer */ - struct baro_report *buf = new struct baro_report[arg]; - - if (nullptr == buf) - return -ENOMEM; - - /* reset the measurement state machine with the new buffer, free the old */ - stop_cycle(); - delete[] _reports; - _num_reports = arg; - _reports = buf; - start_cycle(); + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) + return -EINVAL; - return OK; + irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; } + irqrestore(flags); + return OK; + } case SENSORIOCGQUEUEDEPTH: - return _num_reports - 1; + return _reports->size(); case SENSORIOCRESET: /* XXX implement this */ @@ -469,7 +451,7 @@ MS5611::start_cycle() /* reset the report ring and state machine */ _collect_phase = false; _measure_phase = 0; - _oldest_report = _next_report = 0; + _reports->flush(); /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); @@ -588,8 +570,10 @@ MS5611::collect() perf_begin(_sample_perf); + struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ - _reports[_next_report].timestamp = hrt_absolute_time(); + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); @@ -638,8 +622,8 @@ MS5611::collect() int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; /* generate a new report */ - _reports[_next_report].temperature = _TEMP / 100.0f; - _reports[_next_report].pressure = P / 100.0f; /* convert to millibar */ + report.temperature = _TEMP / 100.0f; + report.pressure = P / 100.0f; /* convert to millibar */ /* altitude calculations based on http://www.kansasflyer.org/index.asp?nav=Avi&sec=Alti&tab=Theory&pg=1 */ @@ -676,18 +660,13 @@ MS5611::collect() * h = ------------------------------- + h1 * a */ - _reports[_next_report].altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; /* publish it */ - orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]); - - /* post a report to the ring - note, not locked */ - INCREMENT(_next_report, _num_reports); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); - /* if we are running up against the oldest report, toss it */ - if (_next_report == _oldest_report) { + if (_reports->force(&report)) { perf_count(_buffer_overflows); - INCREMENT(_oldest_report, _num_reports); } /* notify anyone waiting for data */ @@ -709,8 +688,7 @@ MS5611::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); - printf("report queue: %u (%u/%u @ %p)\n", - _num_reports, _oldest_report, _next_report, _reports); + _reports->print_info("report queue"); printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index f6c624340..e547c913b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -134,6 +134,7 @@ int MS5611_SPI::init() { int ret; + irqstate_t flags; ret = SPI::init(); if (ret != OK) { @@ -261,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg) int MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t flags = irqsave(); - - int ret = transfer(send, recv, len); - - irqrestore(flags); - - return ret; + return transfer(send, recv, len); } #endif /* PX4_SPIDEV_BARO */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index dd475bb6c..3920b5fb8 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -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 @@ -59,11 +59,12 @@ #include <drivers/drv_gpio.h> #include <drivers/drv_hrt.h> -# include <board_config.h> +#include <board_config.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <systemlib/mixer/mixer.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <drivers/drv_mixer.h> #include <drivers/drv_rc_input.h> @@ -72,11 +73,17 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> -#include <systemlib/err.h> + #ifdef HRT_PPM_CHANNEL # include <systemlib/ppm_decode.h> #endif +/* + * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side + */ + +#define CONTROL_INPUT_DROP_LIMIT_MS 20 + class PX4FMU : public device::CDev { public: @@ -100,7 +107,12 @@ public: int set_pwm_alt_channels(uint32_t channels); private: +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) static const unsigned _max_actuators = 4; +#endif +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + static const unsigned _max_actuators = 6; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -117,11 +129,20 @@ private: volatile bool _task_should_exit; bool _armed; + bool _pwm_on; MixerGroup *_mixers; actuator_controls_s _controls; + pwm_limit_t _pwm_limit; + uint16_t _failsafe_pwm[_max_actuators]; + uint16_t _disarmed_pwm[_max_actuators]; + uint16_t _min_pwm[_max_actuators]; + uint16_t _max_pwm[_max_actuators]; + unsigned _num_failsafe_set; + unsigned _num_disarmed_set; + static void task_main_trampoline(int argc, char *argv[]); void task_main() __attribute__((noreturn)); @@ -203,8 +224,18 @@ PX4FMU::PX4FMU() : _primary_pwm_device(false), _task_should_exit(false), _armed(false), - _mixers(nullptr) + _pwm_on(false), + _mixers(nullptr), + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { + for (unsigned i = 0; i < _max_actuators; i++) { + _min_pwm[i] = PWM_MIN; + _max_pwm[i] = PWM_MAX; + } + _debug_enabled = true; } @@ -457,6 +488,9 @@ PX4FMU::task_main() rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; #endif + /* initialize PWM limit lib */ + pwm_limit_init(&_pwm_limit); + log("starting"); /* loop until killed */ @@ -491,90 +525,103 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, 10); + int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); usleep(1000000); continue; - } + } else if (ret == 0) { + /* timeout: no control data, switch to failsafe values */ +// warnx("no PWM: failsafe"); - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - case MODE_4PWM: - num_outputs = 4; - break; - case MODE_6PWM: - num_outputs = 6; - break; - default: - num_outputs = 0; - break; - } + } else { - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - // XXX output actual limited values - memcpy(&controls_effective, &_controls, sizeof(controls_effective)); - - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - isfinite(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; + /* do we have a control update? */ + if (fds[0].revents & POLLIN) { + + /* get controls - must always do this to avoid spinning */ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + case MODE_4PWM: + num_outputs = 4; + break; + case MODE_6PWM: + num_outputs = 6; + break; + default: + num_outputs = 0; + break; } - /* output to the servo */ - up_pwm_servo_set(i, outputs.output[i]); - } + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output actual limited values */ + for (unsigned i = 0; i < num_outputs; i++) { + controls_effective.control_effective[i] = (float)pwm_limited[i]; + } + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* and publish for anyone that cares to see */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + } } - } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* how about an arming update? */ + if (fds[1].revents & POLLIN) { + actuator_armed_s aa; + + /* get new value */ + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + /* update the armed status and check that we're not locked down */ + bool set_armed = aa.armed && !aa.lockdown; + if (_armed != set_armed) + _armed = set_armed; - /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; - if (set_armed != _armed) { - _armed = set_armed; - up_pwm_servo_arm(set_armed); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (aa.armed || _num_disarmed_set > 0); + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } } } @@ -685,14 +732,164 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) up_pwm_servo_arm(false); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_default_rate; + break; + case PWM_SERVO_SET_UPDATE_RATE: ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate; + break; + + case PWM_SERVO_SET_SELECT_UPDATE_RATE: ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = _pwm_alt_rate_channels; + break; + + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_MAX) { + _failsafe_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _failsafe_pwm[i] = PWM_MIN; + } else { + _failsafe_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_failsafe_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_failsafe_pwm[i] > 0) + _num_failsafe_set++; + } + break; + } + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _failsafe_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_MAX) { + _disarmed_pwm[i] = PWM_MAX; + } else if (pwm->values[i] < PWM_MIN) { + _disarmed_pwm[i] = PWM_MIN; + } else { + _disarmed_pwm[i] = pwm->values[i]; + } + } + + /* + * update the counter + * this is needed to decide if disarmed PWM output should be turned on or not + */ + _num_disarmed_set = 0; + for (unsigned i = 0; i < _max_actuators; i++) { + if (_disarmed_pwm[i] > 0) + _num_disarmed_set++; + } + break; + } + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _disarmed_pwm[i]; + } + pwm->channel_count = _max_actuators; + break; + } + + case PWM_SERVO_SET_MIN_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] > PWM_HIGHEST_MIN) { + _min_pwm[i] = PWM_HIGHEST_MIN; + } else if (pwm->values[i] < PWM_MIN) { + _min_pwm[i] = PWM_MIN; + } else { + _min_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _min_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + + case PWM_SERVO_SET_MAX_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + /* discard if too many values are sent */ + if (pwm->channel_count > _max_actuators) { + ret = -EINVAL; + break; + } + for (unsigned i = 0; i < pwm->channel_count; i++) { + if (pwm->values[i] == 0) { + /* ignore 0 */ + } else if (pwm->values[i] < PWM_LOWEST_MAX) { + _max_pwm[i] = PWM_LOWEST_MAX; + } else if (pwm->values[i] > PWM_MAX) { + _max_pwm[i] = PWM_MAX; + } else { + _max_pwm[i] = pwm->values[i]; + } + } + break; + } + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values*)arg; + for (unsigned i = 0; i < _max_actuators; i++) { + pwm->values[i] = _max_pwm[i]; + } + pwm->channel_count = _max_actuators; + arg = (unsigned long)&pwm; + break; + } + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1110,10 +1307,11 @@ fmu_stop(void) void test(void) { - int fd; + int fd; unsigned servo_count = 0; unsigned pwm_value = 1000; int direction = 1; + int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -1128,9 +1326,9 @@ test(void) warnx("Testing %u servos", (unsigned)servo_count); - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -1149,7 +1347,7 @@ test(void) } } else { // and use write interface for the other direction - int ret = write(fd, servos, sizeof(servos)); + ret = write(fd, servos, sizeof(servos)); if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); } @@ -1180,15 +1378,17 @@ test(void) /* Check if user wants to quit */ char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); break; } } } - close(console); close(fd); exit(0); diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index 05bc7a5b3..d918abd57 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,4 +3,5 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp +SRCS = fmu.cpp \ + ../../modules/systemlib/pwm_limit/pwm_limit.c diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bd5f33043..63e775857 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,7 +80,9 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/servorail_status.h> #include <uORB/topics/parameter_update.h> + #include <debug.h> #include <mavlink/mavlink_log.h> @@ -94,6 +96,10 @@ extern device::Device *PX4IO_serial_interface() weak_function; #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +#define UPDATE_INTERVAL_MIN 2 // 2 ms -> 500 Hz +#define ORB_CHECK_INTERVAL 200000 // 200 ms -> 5 Hz +#define IO_POLL_INTERVAL 20000 // 20 ms -> 50 Hz + /** * The PX4IO class. * @@ -104,35 +110,35 @@ class PX4IO : public device::CDev public: /** * Constructor. - * + * * Initialize all class variables. */ PX4IO(device::Device *interface); /** * Destructor. - * + * * Wait for worker thread to terminate. */ virtual ~PX4IO(); /** * Initialize the PX4IO class. - * + * * Retrieve relevant initial system parameters. Initialize PX4IO registers. */ virtual int init(); /** * Detect if a PX4IO is connected. - * + * * Only validate if there is a PX4IO to talk to. */ virtual int detect(); /** * IO Control handler. - * + * * Handle all IOCTL calls to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -143,7 +149,7 @@ public: /** * write handler. - * + * * Handle writes to the PX4IO file descriptor. * * @param[in] filp file handle (not used). This function is always called directly through object reference @@ -178,6 +184,11 @@ public: int set_failsafe_values(const uint16_t *vals, unsigned len); /** + * Disable RC input handling + */ + int disable_rc_handling(); + + /** * Print IO status. * * Print all relevant IO status information @@ -185,17 +196,17 @@ public: void print_status(); /** - * Disable RC input handling + * Fetch and print debug console output. */ - int disable_rc_handling(); + int print_debug(); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Set the DSM VCC is controlled by relay one flag * * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline void set_dsm_vcc_ctl(bool enable) - { + inline void set_dsm_vcc_ctl(bool enable) { _dsm_vcc_ctl = enable; }; @@ -204,10 +215,12 @@ public: * * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled */ - inline bool get_dsm_vcc_ctl() - { + inline bool get_dsm_vcc_ctl() { return _dsm_vcc_ctl; }; +#endif + + inline uint16_t system_status() const {return _status;} private: device::Device *_interface; @@ -226,7 +239,8 @@ private: volatile int _task; ///<worker task id volatile bool _task_should_exit; ///<worker terminate flag - int _mavlink_fd; ///<mavlink file descriptor + int _mavlink_fd; ///<mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread. + int _thread_mavlink_fd; ///<mavlink file descriptor for thread. perf_counter_t _perf_update; ///<local performance counter @@ -239,12 +253,14 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///<mixed outputs @@ -257,7 +273,9 @@ private: float _battery_mamphour_total;///<amp hours consumed so far uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power +#endif /** * Trampoline to the worker task @@ -374,44 +392,55 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen); + int mixer_send(const char *buf, unsigned buflen, unsigned retries = 3); /** - * Set the minimum PWM signals when armed + * Handle a status update from IO. + * + * Publish IO status information if necessary. + * + * @param status The status register */ - int set_min_values(const uint16_t *vals, unsigned len); + int io_handle_status(uint16_t status); /** - * Set the maximum PWM signal when armed + * Handle an alarm update from IO. + * + * Publish IO alarm information if necessary. + * + * @param alarm The status register */ - int set_max_values(const uint16_t *vals, unsigned len); + int io_handle_alarms(uint16_t alarms); /** - * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + * Handle issuing dsm bind ioctl to px4io. + * + * @param dsmMode 0:dsm2, 1:dsmx */ - int set_idle_values(const uint16_t *vals, unsigned len); + void dsm_bind_ioctl(int dsmMode); /** - * Handle a status update from IO. + * Handle a battery update from IO. * - * Publish IO status information if necessary. + * Publish IO battery information if necessary. * - * @param status The status register + * @param vbatt vbatt register + * @param ibatt ibatt register */ - int io_handle_status(uint16_t status); + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); /** - * Handle an alarm update from IO. + * Handle a servorail update from IO. * - * Publish IO alarm information if necessary. + * Publish servo rail information if necessary. * - * @param alarm The status register + * @param vservo vservo register + * @param vrssi vrssi register */ - int io_handle_alarms(uint16_t alarms); + void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); }; - namespace { @@ -433,6 +462,7 @@ PX4IO::PX4IO(device::Device *interface) : _task(-1), _task_should_exit(false), _mavlink_fd(-1), + _thread_mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -440,17 +470,22 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_servorail(0), _to_safety(0), _primary_pwm_device(false), - _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor + _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0), - _dsm_vcc_ctl(false) + _battery_last_timestamp(0) +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + , _dsm_vcc_ctl(false) +#endif + { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -487,25 +522,30 @@ PX4IO::detect() { int ret; - ASSERT(_task == -1); + if (_task == -1) { - /* do regular cdev init */ - ret = CDev::init(); - if (ret != OK) - return ret; + /* do regular cdev init */ + ret = CDev::init(); - /* get some parameters */ - unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); - if (protocol != PX4IO_PROTOCOL_VERSION) { - if (protocol == _io_reg_get_error) { - log("IO not installed"); - } else { - log("IO version error"); - mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + if (ret != OK) + return ret; + + /* get some parameters */ + unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + + if (protocol != PX4IO_PROTOCOL_VERSION) { + if (protocol == _io_reg_get_error) { + log("IO not installed"); + + } else { + log("IO version error"); + mavlink_log_emergency(_mavlink_fd, "IO VERSION MISMATCH, PLEASE UPGRADE SOFTWARE!"); + } + + return -1; } - - return -1; } + log("IO found"); return 0; @@ -520,22 +560,26 @@ PX4IO::init() /* do regular cdev init */ ret = CDev::init(); + if (ret != OK) return ret; /* get some parameters */ unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + if (protocol != PX4IO_PROTOCOL_VERSION) { log("protocol/firmware mismatch"); mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort."); return -1; } + _hardware = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION); _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); + if ((_max_actuators < 1) || (_max_actuators > 255) || (_max_relays > 32) || (_max_transfer < 16) || (_max_transfer > 255) || @@ -545,6 +589,7 @@ PX4IO::init() mavlink_log_emergency(_mavlink_fd, "[IO] config read fail, abort."); return -1; } + if (_max_rc_input > RC_INPUT_MAX_CHANNELS) _max_rc_input = RC_INPUT_MAX_CHANNELS; @@ -559,6 +604,7 @@ PX4IO::init() /* get IO's last seen FMU state */ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); + if (ret != OK) return ret; @@ -569,8 +615,11 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); - log("INAIR RESTART RECOVERY (needs commander app running)"); + /* get a status update from IO */ + io_get_status(); + + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -582,7 +631,7 @@ PX4IO::init() struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { @@ -598,7 +647,7 @@ PX4IO::init() usleep(10000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 3000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 3000) { log("failed to recover from in-air restart (1), aborting IO driver init."); return 1; } @@ -638,7 +687,7 @@ PX4IO::init() usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { + if ((hrt_absolute_time() - try_start_time) / 1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } @@ -649,25 +698,27 @@ PX4IO::init() log("re-sending arm cmd"); } - /* keep waiting for state change for 2 s */ + /* keep waiting for state change for 2 s */ } while (!safety.armed); - /* regular boot, no in-air restart, init IO */ - } else { + /* regular boot, no in-air restart, init IO */ + } else { /* dis-arm IO before touching anything */ - io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, - PX4IO_P_SETUP_ARMING_FMU_ARMED | - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); + io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, + PX4IO_P_SETUP_ARMING_FMU_ARMED | + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); if (_rc_handling_disabled) { ret = io_disable_rc_handling(); + } else { /* publish RC config to IO */ ret = io_set_rc_config(); + if (ret != OK) { log("failed to update RC input config"); mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); @@ -707,11 +758,12 @@ PX4IO::task_main_trampoline(int argc, char *argv[]) void PX4IO::task_main() { - hrt_abstime last_poll_time = 0; - int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + hrt_abstime poll_last = 0; + hrt_abstime orb_check_last = 0; log("starting"); + _thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -720,36 +772,26 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ - _t_param = orb_subscribe(ORB_ID(parameter_update)); - orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); if ((_t_actuators < 0) || - (_t_actuator_armed < 0) || - (_t_vehicle_control_mode < 0) || - (_t_param < 0)) { + (_t_actuator_armed < 0) || + (_t_vehicle_control_mode < 0) || + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[1]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; - fds[2].fd = _t_vehicle_control_mode; - fds[2].events = POLLIN; - fds[3].fd = _t_param; - fds[3].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -759,17 +801,19 @@ PX4IO::task_main() /* adjust update interval */ if (_update_interval != 0) { - if (_update_interval < 5) - _update_interval = 5; + if (_update_interval < UPDATE_INTERVAL_MIN) + _update_interval = UPDATE_INTERVAL_MIN; + if (_update_interval > 100) _update_interval = 100; + orb_set_interval(_t_actuators, _update_interval); _update_interval = 0; } /* sleep waiting for topic updates, but no more than 20ms */ unlock(); - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20); + int ret = ::poll(fds, 1, 20); lock(); /* this would be bad... */ @@ -782,67 +826,79 @@ PX4IO::task_main() hrt_abstime now = hrt_absolute_time(); /* if we have new control data from the ORB, handle it */ - if (fds[0].revents & POLLIN) + if (fds[0].revents & POLLIN) { io_set_control_state(); + } - /* if we have an arming state update, handle it */ - if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) - io_set_arming_state(); - - /* - * If it's time for another tick of the polling status machine, - * try it now. - */ - if ((now - last_poll_time) >= 20000) { + if (now >= poll_last + IO_POLL_INTERVAL) { + /* run at 50Hz */ + poll_last = now; - /* - * Pull status and alarms from IO. - */ + /* pull status and alarms from IO */ io_get_status(); - /* - * Get raw R/C input from IO. - */ + /* get raw R/C input from IO */ io_publish_raw_rc(); - /* - * Fetch mixed servo controls and PWM outputs from IO. - * - * XXX We could do this at a reduced rate in many/most cases. - */ + /* fetch mixed servo controls and PWM outputs from IO */ io_publish_mixed_controls(); io_publish_pwm_outputs(); + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* check updates on uORB topics and handle it */ + bool updated = false; + + /* arming state */ + orb_check(_t_actuator_armed, &updated); + + if (!updated) { + orb_check(_t_vehicle_control_mode, &updated); + } + + if (updated) { + io_set_arming_state(); + } + + /* vehicle command */ + orb_check(_t_vehicle_command, &updated); + + if (updated) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + + // Check for a DSM pairing command + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1 == 0.0f)) { + dsm_bind_ioctl((int)cmd.param2); + } + } /* * If parameters have changed, re-send RC mappings to IO * * XXX this may be a bit spammy */ - if (fds[3].revents & POLLIN) { + orb_check(_t_param, &updated); + + if (updated) { parameter_update_s pupdate; + orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); + int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to 0 + /* see if bind parameter has been set, and reset it to -1 */ param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val) { - if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { - mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); - } else { - mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); - } - } else { - mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); - } - dsm_bind_val = 0; + + if (dsm_bind_val > -1) { + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); } - /* copy to reset the notification */ - orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); - /* re-upload RC input config as it may have changed */ io_set_rc_config(); } @@ -873,7 +929,7 @@ PX4IO::io_set_control_state() /* get controls */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &controls); + ORB_ID(actuator_controls_1), _t_actuators, &controls); for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); @@ -882,55 +938,6 @@ PX4IO::io_set_control_state() return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } -int -PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) -{ - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); -} - -int -PX4IO::set_min_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); -} - -int -PX4IO::set_max_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); -} - -int -PX4IO::set_idle_values(const uint16_t *vals, unsigned len) -{ - - if (len > _max_actuators) - /* fail with error */ - return E2BIG; - - printf("Sending IDLE values\n"); - - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); -} - int PX4IO::io_set_arming_state() @@ -946,17 +953,21 @@ PX4IO::io_set_arming_state() if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; + } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } @@ -1001,22 +1012,27 @@ PX4IO::io_set_rc_config() * autopilots / GCS'. */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan >= 0) && (ichan < (int)_max_rc_input)) input_map[ichan - 1] = 3; ichan = 4; + for (unsigned i = 0; i < _max_rc_input; i++) if (input_map[i] == -1) input_map[i] = ichan++; @@ -1071,6 +1087,7 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + if (ret != OK) { log("rc config upload failed"); break; @@ -1098,13 +1115,14 @@ PX4IO::io_handle_status(uint16_t status) /* check for IO reset - force it back to armed if necessary */ if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + && !(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_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; + } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -1128,6 +1146,7 @@ PX4IO::io_handle_status(uint16_t status) if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; + } else { safety.safety_off = false; safety.safety_switch_available = true; @@ -1136,6 +1155,7 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { _to_safety = orb_advertise(ORB_ID(safety), &safety); } @@ -1143,6 +1163,24 @@ PX4IO::io_handle_status(uint16_t status) return ret; } +void +PX4IO::dsm_bind_ioctl(int dsmMode) +{ + if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* 0: dsm2, 1:dsmx */ + if ((dsmMode == 0) || (dsmMode == 1)) { + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); + } + + } else { + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + } +} + + int PX4IO::io_handle_alarms(uint16_t alarms) { @@ -1156,53 +1194,88 @@ PX4IO::io_handle_alarms(uint16_t alarms) return 0; } +void +PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) +{ + /* only publish if battery has a valid minimum voltage */ + if (vbatt <= 3300) { + return; + } + + battery_status_s battery_status; + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = vbatt / 1000.0f; + + /* + ibatt contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; + + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } +} + +void +PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) +{ + servorail_status_s servorail_status; + servorail_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + servorail_status.voltage_v = vservo * 0.001f; + servorail_status.rssi_v = vrssi * 0.001f; + + /* lazily publish the servorail voltages */ + if (_to_servorail > 0) { + orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + + } else { + _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + } +} + int PX4IO::io_get_status() { - uint16_t regs[4]; + uint16_t regs[6]; int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); + if (ret != OK) return ret; io_handle_status(regs[0]); io_handle_alarms(regs[1]); - - /* only publish if battery has a valid minimum voltage */ - if (regs[2] > 3300) { - battery_status_s battery_status; - - battery_status.timestamp = hrt_absolute_time(); - /* voltage is scaled to mV */ - battery_status.voltage_v = regs[2] / 1000.0f; - - /* - regs[3] contains the raw ADC count, as 12 bit ADC - value, with full range being 3.3v - */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; - battery_status.current_a += _battery_amp_bias; - - /* - integrate battery over time to get total mAh used - */ - if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; - } - battery_status.discharged_mah = _battery_mamphour_total; - _battery_last_timestamp = battery_status.timestamp; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + io_handle_vservo(regs[4], regs[5]); +#endif return ret; } @@ -1215,7 +1288,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - + static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; @@ -1226,6 +1299,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ input_rc.timestamp = hrt_absolute_time(); ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); + if (ret != OK) return ret; @@ -1234,8 +1308,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * channel count once. */ channel_count = regs[0]; + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); + if (ret != OK) return ret; } @@ -1258,16 +1334,20 @@ PX4IO::io_publish_raw_rc() rc_val.timestamp = hrt_absolute_time(); int ret = io_get_raw_rc_input(rc_val); + if (ret != OK) return ret; /* sort out the source of the values */ if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + } else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } @@ -1275,7 +1355,8 @@ PX4IO::io_publish_raw_rc() /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); - } else { + + } else { orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val); } @@ -1300,6 +1381,7 @@ PX4IO::io_publish_mixed_controls() /* get actuator controls from IO */ uint16_t act[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators); + if (ret != OK) return ret; @@ -1309,16 +1391,17 @@ PX4IO::io_publish_mixed_controls() /* laxily advertise on first publication */ if (_to_actuators_effective == 0) { - _to_actuators_effective = - orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - &controls_effective); + _to_actuators_effective = + orb_advertise((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + &controls_effective); + } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : - ORB_ID(actuator_controls_effective_1)), - _to_actuators_effective, &controls_effective); + orb_publish((_primary_pwm_device ? + ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : + ORB_ID(actuator_controls_effective_1)), + _to_actuators_effective, &controls_effective); } return OK; @@ -1338,26 +1421,29 @@ PX4IO::io_publish_pwm_outputs() /* get servo values from IO */ uint16_t ctl[_max_actuators]; int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators); + if (ret != OK) return ret; /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) outputs.output[i] = ctl[i]; + outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ if (_to_outputs == 0) { _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + &outputs); + } else { orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + ORB_ID_VEHICLE_CONTROLS : + ORB_ID(actuator_outputs_1)), + _to_outputs, + &outputs); } return OK; @@ -1373,10 +1459,12 @@ PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned } int ret = _interface->write((page << 8) | offset, (void *)values, num_values); + if (ret != (int)num_values) { debug("io_reg_set(%u,%u,%u): error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1396,10 +1484,12 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v } int ret = _interface->read((page << 8) | offset, reinterpret_cast<void *>(values), num_values); + if (ret != (int)num_values) { debug("io_reg_get(%u,%u,%u): data error %d", page, offset, num_values, ret); return -1; } + return OK; } @@ -1421,8 +1511,10 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); + if (ret != OK) return ret; + value &= ~clearbits; value |= setbits; @@ -1430,61 +1522,135 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int -PX4IO::mixer_send(const char *buf, unsigned buflen) +PX4IO::print_debug() { - uint8_t frame[_max_transfer]; - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + int io_fd = -1; - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; + if (io_fd < 0) { + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); + } - do { - unsigned count = buflen; + /* read IO's output */ + if (io_fd > 0) { + pollfd fds[1]; + fds[0].fd = io_fd; + fds[0].events = POLLIN; - if (count > max_len) - count = max_len; + usleep(500); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - } + if (pret > 0) { + int count; + char buf[65]; - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { - msg->text[count] = '\0'; - total_len++; - } + do { + count = ::read(io_fd, buf, sizeof(buf) - 1); - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + if (count > 0) { + /* enforce null termination */ + buf[count] = '\0'; + warnx("IO CONSOLE: %s", buf); + } - if (ret) { - log("mixer send error %d", ret); - return ret; + } while (count > 0); } - msg->action = F2I_MIXER_ACTION_APPEND; + ::close(io_fd); + return 0; + } + +#endif + return 1; + +} + +int +PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) +{ + /* get debug level */ + int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); + + uint8_t frame[_max_transfer]; + + do { + + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); + + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; + + do { + unsigned count = buflen; + + if (count > max_len) + count = max_len; + + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + + } else { + continue; + } + + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + + if (total_len % 2) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + /* print mixer chunk */ + if (debuglevel > 5 || ret) { + + warnx("fmu sent: \"%s\"", msg->text); - } while (buflen > 0); + /* read IO's output */ + print_debug(); + } + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; + + } while (buflen > 0); + + /* ensure a closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + retries--; + + log("mixer sent"); + + } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - debug("mixer upload OK"); mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; - } else { - debug("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } + log("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ return -EINVAL; } @@ -1494,48 +1660,48 @@ PX4IO::print_status() { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_HARDWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), - io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); /* status */ printf("%u bytes free\n", - io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", - flags, - ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), - ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); + flags, + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), + ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s%s%s\n", - alarms, - ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_VSERVO_FAULT) ? " VSERVO_FAULT" : "")); /* now clear alarms */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, 0xFFFF); @@ -1548,79 +1714,107 @@ PX4IO::print_status() (double)_battery_amp_per_volt, (double)_battery_amp_bias, (double)_battery_mamphour_total); + } else if (_hardware == 2) { printf("vservo %u mV vservo scale %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VSERVO), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VSERVO_SCALE)); printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI)); } + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } + printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", - arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + printf("rates 0x%04x default %u alt %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); +#endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\nidle values"); + + printf("\ndisarmed values"); + for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i)); + printf("\n"); } @@ -1652,44 +1846,119 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0); break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + /* get the default update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested alternate rate */ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); break; - case PWM_SERVO_SELECT_UPDATE_RATE: { - - /* blindly clear the PWM update alarm - might be set for some other reason */ - io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + case PWM_SERVO_GET_UPDATE_RATE: + /* get the alternative update rate */ + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE); + break; - /* attempt to set the rate map */ - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + case PWM_SERVO_SET_SELECT_UPDATE_RATE: { - /* check that the changes took */ - uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { - ret = -EINVAL; + /* blindly clear the PWM update alarm - might be set for some other reason */ io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { + ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + } + + break; } + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + + *(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES); + break; + + case PWM_SERVO_SET_FAILSAFE_PWM: { + struct pwm_output_values* pwm = (struct pwm_output_values*)arg; + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); break; } + case PWM_SERVO_GET_FAILSAFE_PWM: + + ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } + break; + case PWM_SERVO_SET_DISARMED_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_idle_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_DISARMED_PWM: + + ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_SET_MIN_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_min_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_MIN_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_SET_MAX_PWM: { struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - set_max_values(pwm->values, pwm->channel_count); + if (pwm->channel_count > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); + break; } + + case PWM_SERVO_GET_MAX_PWM: + + ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators); + if (ret != OK) { + ret = -EIO; + } break; case PWM_SERVO_GET_COUNT: @@ -1697,11 +1966,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case DSM_BIND_START: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(50000); + usleep(72000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); @@ -1713,77 +1982,115 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { - /* TODO: we could go lower for e.g. TurboPWM */ - unsigned channel = cmd - PWM_SERVO_SET(0); - if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { - ret = -EINVAL; - } else { - /* send a direct PWM value */ - ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); - } + /* TODO: we could go lower for e.g. TurboPWM */ + unsigned channel = cmd - PWM_SERVO_SET(0); - break; - } + if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) { + ret = -EINVAL; + + } else { + /* send a direct PWM value */ + ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg); + } + + break; + } case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET(0); + unsigned channel = cmd - PWM_SERVO_GET(0); + + if (channel >= _max_actuators) { + ret = -EINVAL; - if (channel >= _max_actuators) { - ret = -EINVAL; - } else { - /* fetch a current PWM value */ - uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); - if (value == _io_reg_get_error) { - ret = -EIO; } else { - *(servo_position_t *)arg = value; + /* fetch a current PWM value */ + uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel); + + if (value == _io_reg_get_error) { + ret = -EIO; + + } else { + *(servo_position_t *)arg = value; + } } + + break; } - break; - } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); - if (*(uint32_t *)arg == _io_reg_get_error) - ret = -EIO; - break; - } + *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + if (*(uint32_t *)arg == _io_reg_get_error) + ret = -EIO; + + break; + } case GPIO_RESET: { - uint32_t bits = (1 << _max_relays) - 1; - /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl) - bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); - break; - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + uint32_t bits = (1 << _max_relays) - 1; + + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif + break; + } case GPIO_SET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + } + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_CLEAR: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); + /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + break; + } + + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_GET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); + if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; + +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case MIXERIOCGETOUTPUTCOUNT: @@ -1795,40 +2102,44 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - ret = mixer_send(buf, strnlen(buf, 2048)); - break; - } + const char *buf = (const char *)arg; + ret = mixer_send(buf, strnlen(buf, 2048)); + break; + } case RC_INPUT_GET: { - uint16_t status; - rc_input_values *rc_val = (rc_input_values *)arg; + uint16_t status; + rc_input_values *rc_val = (rc_input_values *)arg; - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - if (ret != OK) - break; + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1); - /* if no R/C input, don't try to fetch anything */ - if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { - ret = -ENOTCONN; - break; - } + if (ret != OK) + break; - /* sort out the source of the values */ - if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; - } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { - rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; - } else { - rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; - } + /* if no R/C input, don't try to fetch anything */ + if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) { + ret = -ENOTCONN; + break; + } - /* read raw R/C input values */ - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); - break; - } + /* sort out the source of the values */ + if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM; + + } else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) { + rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS; + + } else { + rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN; + } + + /* read raw R/C input values */ + ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input); + break; + } case PX4IO_SET_DEBUG: /* set the debug level */ @@ -1836,12 +2147,15 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ if (arg) { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); } + break; default: @@ -1860,11 +2174,14 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len) if (count > _max_actuators) count = _max_actuators; + if (count > 0) { int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count); + if (ret != OK) return ret; } + return count * 2; } @@ -1872,8 +2189,9 @@ int PX4IO::set_update_rate(int rate) { int interval_ms = 1000 / rate; - if (interval_ms < 3) { - interval_ms = 3; + + if (interval_ms < UPDATE_INTERVAL_MIN) { + interval_ms = UPDATE_INTERVAL_MIN; warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); } @@ -1904,22 +2222,27 @@ get_interface() device::Device *interface = nullptr; #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* try for a serial interface */ if (PX4IO_serial_interface != nullptr) interface = PX4IO_serial_interface(); + if (interface != nullptr) goto got; + #endif /* try for an I2C interface if we haven't got a serial one */ if (PX4IO_i2c_interface != nullptr) interface = PX4IO_i2c_interface(); + if (interface != nullptr) goto got; errx(1, "cannot alloc interface"); got: + if (interface->init() != OK) { delete interface; errx(1, "interface init failed"); @@ -1953,7 +2276,7 @@ start(int argc, char *argv[]) if (argc > 1) { if (!strcmp(argv[1], "norc")) { - if(g_dev->disable_rc_handling()) + if (g_dev->disable_rc_handling()) warnx("Failed disabling RC handling"); } else { @@ -1961,6 +2284,7 @@ start(int argc, char *argv[]) } } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { @@ -1969,6 +2293,8 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } + +#endif exit(0); } @@ -1995,6 +2321,7 @@ detect(int argc, char *argv[]) if (ret) { /* nonzero, error */ exit(1); + } else { exit(0); } @@ -2008,21 +2335,33 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); +#endif + if (argc < 3) errx(0, "needs argument, use dsm2 or dsmx"); if (!strcmp(argv[2], "dsm2")) - pulses = 3; + pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) - pulses = 7; + pulses = DSMX_BIND_PULSES; + else if (!strcmp(argv[2], "dsmx8")) + pulses = DSMX8_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + // Test for custom pulse parameter + if (argc > 3) + pulses = atoi(argv[3]); + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + errx(1, "system must not be armed"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - +#endif g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); @@ -2048,16 +2387,16 @@ test(void) /* tell IO that its ok to disable its safety with the switch */ ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) err(1, "PWM_SERVO_SET_ARM_OK"); if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); @@ -2065,22 +2404,27 @@ test(void) /* sweep all servos between 1000..2000 */ servo_position_t servos[servo_count]; + for (unsigned i = 0; i < servo_count; i++) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { if (pwm_value < 2000) { pwm_value++; + } else { direction = -1; } + } else { if (pwm_value > 1000) { pwm_value--; + } else { direction = 1; } @@ -2092,16 +2436,21 @@ test(void) if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) err(1, "error reading PWM servo %d", i); + if (value != servos[i]) errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); } /* Check if user wants to quit */ char c; - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); - close(console); exit(0); } } @@ -2111,28 +2460,38 @@ test(void) void monitor(void) { + /* clear screen */ + printf("\033[2J"); + unsigned cancels = 3; - printf("Hit <enter> three times to exit monitor mode\n"); for (;;) { pollfd fds[1]; fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 500); + poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); - if (cancels-- == 0) + if (cancels-- == 0) { + printf("\033[H"); /* move cursor home and clear screen */ exit(0); + } } -#warning implement this + if (g_dev != nullptr) { + + printf("\033[H"); /* move cursor home and clear screen */ + (void)g_dev->print_status(); + (void)g_dev->print_debug(); + printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); -// if (g_dev != nullptr) -// g_dev->dump_one = true; + } else { + errx(1, "driver not loaded, exiting"); + } } } @@ -2171,7 +2530,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[5]; + const char *fn[3]; /* work out what we're uploading... */ if (argc > 2) { @@ -2179,11 +2538,19 @@ px4io_main(int argc, char *argv[]) fn[1] = nullptr; } else { +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) + fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[1] = "/fs/microsd/px4io1.bin"; + fn[2] = "/fs/microsd/px4io.bin"; + fn[3] = nullptr; +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) fn[0] = "/etc/extras/px4io-v2_default.bin"; - fn[1] = "/etc/extras/px4io-v1_default.bin"; + fn[1] = "/fs/microsd/px4io2.bin"; fn[2] = "/fs/microsd/px4io.bin"; - fn[3] = "/fs/microsd/px4io2.bin"; - fn[4] = nullptr; + fn[3] = nullptr; +#else +#error "unknown board" +#endif } up = new PX4IO_Uploader; @@ -2230,164 +2597,35 @@ px4io_main(int argc, char *argv[]) if ((argc > 2)) { g_dev->set_update_rate(atoi(argv[2])); + } else { - errx(1, "missing argument (50 - 400 Hz)"); + errx(1, "missing argument (50 - 500 Hz)"); return 1; } + exit(0); } if (!strcmp(argv[1], "current")) { if ((argc > 3)) { g_dev->set_battery_current_scaling(atof(argv[2]), atof(argv[3])); + } else { errx(1, "missing argument (apm_per_volt, amp_offset)"); return 1; } - exit(0); - } - - if (!strcmp(argv[1], "failsafe")) { - - if (argc < 3) { - errx(1, "failsafe command needs at least one channel value (ppm)"); - } - - /* set values for first 8 channels, fill unassigned channels with 1500. */ - uint16_t failsafe[8]; - - for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) { - - /* set channel to commandline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - failsafe[i] = atoi(argv[i+2]); - if (failsafe[i] < 800 || failsafe[i] > 2200) { - errx(1, "value out of range of 800 < value < 2200. Aborting."); - } - } else { - /* a zero value will result in stopping to output any pulse */ - failsafe[i] = 0; - } - } - - int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); - - if (ret != OK) - errx(ret, "failed setting failsafe values"); - exit(0); - } - - if (!strcmp(argv[1], "min")) { - - if (argc < 3) { - errx(1, "min command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 900 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 1200) { - errx(1, "value out of range of 900 < value < 1200. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting min values"); - } else { - errx(1, "not loaded"); - } exit(0); } - if (!strcmp(argv[1], "max")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - if (iofd > 0) { - - pwm.channel_count = 0; - - for (int i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 2100 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 1800 || pwm.values[i] > 2100) { - errx(1, "value out of range of 1800 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting max values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } - - if (!strcmp(argv[1], "idle") || !strcmp(argv[1], "disarmed")) { - - if (argc < 3) { - errx(1, "max command needs at least one channel value (PWM)"); - } - - int iofd = open(PWM_OUTPUT_DEVICE_PATH, 0); - struct pwm_output_values pwm; - - if (iofd > 0) { - - pwm.channel_count = 0; - - for (unsigned i = 0; i < sizeof(pwm.values) / sizeof(pwm.values[0]); i++) - { - /* set channel to commanline argument or to 0 for non-provided channels */ - if (argc > i + 2) { - pwm.values[i] = atoi(argv[i+2]); - if (pwm.values[i] < 900 || pwm.values[i] > 2100) { - errx(1, "value out of range of 900 < value < 2100. Aborting."); - } - pwm.channel_count++; - } - } - - int ret = ioctl(iofd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm); - - if (ret != OK) - errx(ret, "failed setting idle values"); - } else { - errx(1, "not loaded"); - } - exit(0); - } if (!strcmp(argv[1], "recovery")) { /* * Enable in-air restart support. * We can cheat and call the driver directly, as it - * doesn't reference filp in ioctl() + * doesn't reference filp in ioctl() */ g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); exit(0); @@ -2414,19 +2652,23 @@ px4io_main(int argc, char *argv[]) printf("usage: px4io debug LEVEL\n"); exit(1); } + if (g_dev == nullptr) { printf("px4io is not started\n"); exit(1); } + uint8_t level = atoi(argv[2]); /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); + if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); } + printf("SET_DEBUG %u OK\n", (unsigned)level); exit(0); } @@ -2447,6 +2689,6 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); - out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); +out: + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'"); } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 236cb918d..43318ca84 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -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 */
\ No newline at end of file +#endif /* PX4IO_SERIAL_BASE */ diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fe8561a0b..d01dedb0d 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) fds[0].fd = _io_fd; fds[0].events = POLLIN; - /* wait 100 ms for a character */ + /* wait <timout> ms for a character */ int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index feb8f1c6c..727c86e02 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -1,6 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,7 +38,6 @@ * * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. * - * */ #include <nuttx/config.h> @@ -92,53 +93,51 @@ public: private: work_s _work; - rgbled_color_t _color; rgbled_mode_t _mode; rgbled_pattern_t _pattern; - float _brightness; uint8_t _r; uint8_t _g; uint8_t _b; + float _brightness; - bool _should_run; bool _running; int _led_interval; + bool _should_run; int _counter; void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); - void set_brightness(float brightness); static void led_trampoline(void *arg); void led(); - int set(bool on, uint8_t r, uint8_t g, uint8_t b); - int set_on(bool on); - int set_rgb(uint8_t r, uint8_t g, uint8_t b); - int get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b); + int send_led_enable(bool enable); + int send_led_rgb(); + int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b); }; /* for now, we only support one RGBLED */ namespace { - RGBLED *g_rgbled; +RGBLED *g_rgbled; } +void rgbled_usage(); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), - _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), - _running(false), - _brightness(1.0f), _r(0), _g(0), _b(0), + _brightness(1.0f), + _running(false), _led_interval(0), + _should_run(false), _counter(0) { memset(&_work, 0, sizeof(_work)); @@ -159,8 +158,9 @@ RGBLED::init() return ret; } - /* start off */ - set(false, 0, 0, 0); + /* switch off LED on start */ + send_led_enable(false); + send_led_rgb(); return OK; } @@ -169,10 +169,23 @@ int RGBLED::probe() { int ret; - bool on, not_powersave; + bool on, powersave; uint8_t r, g, b; - ret = get(on, not_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; } @@ -181,15 +194,16 @@ int RGBLED::info() { int ret; - bool on, not_powersave; + bool on, powersave; uint8_t r, g, b; - ret = get(on, not_powersave, r, g, b); + ret = get(on, powersave, r, g, b); if (ret == OK) { /* we don't care about power-save mode */ log("state: %s", on ? "ON" : "OFF"); log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); + } else { warnx("failed to read led"); } @@ -201,28 +215,30 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; + switch (cmd) { case RGBLED_SET_RGB: - /* set the specified RGB values */ - rgbled_rgbset_t rgbset; - memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset)); - set_rgb(rgbset.red, rgbset.green, rgbset.blue); - set_mode(RGBLED_MODE_ON); + /* set the specified color */ + _r = ((rgbled_rgbset_t *) arg)->red; + _g = ((rgbled_rgbset_t *) arg)->green; + _b = ((rgbled_rgbset_t *) arg)->blue; + send_led_rgb(); return OK; case RGBLED_SET_COLOR: /* set the specified color name */ set_color((rgbled_color_t)arg); + send_led_rgb(); return OK; case RGBLED_SET_MODE: - /* set the specified blink speed */ + /* set the specified mode */ set_mode((rgbled_mode_t)arg); return OK; case RGBLED_SET_PATTERN: /* set a special pattern */ - set_pattern((rgbled_pattern_t*)arg); + set_pattern((rgbled_pattern_t *)arg); return OK; default: @@ -241,39 +257,59 @@ RGBLED::led_trampoline(void *arg) rgbl->led(); } - - +/** + * Main loop function + */ void RGBLED::led() { + if (!_should_run) { + _running = false; + return; + } + switch (_mode) { - case RGBLED_MODE_BLINK_SLOW: - case RGBLED_MODE_BLINK_NORMAL: - case RGBLED_MODE_BLINK_FAST: - if(_counter % 2 == 0) - set_on(true); - else - set_on(false); - break; - case RGBLED_MODE_BREATHE: - if (_counter >= 30) - _counter = 0; - if (_counter <= 15) { - set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); - } else { - set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); - } - break; - case RGBLED_MODE_PATTERN: - /* don't run out of the pattern array and stop if the next frame is 0 */ - if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) - _counter = 0; + case RGBLED_MODE_BLINK_SLOW: + case RGBLED_MODE_BLINK_NORMAL: + case RGBLED_MODE_BLINK_FAST: + if (_counter >= 2) + _counter = 0; - set_color(_pattern.color[_counter]); - _led_interval = _pattern.duration[_counter]; - break; - default: - break; + send_led_enable(_counter == 0); + + break; + + case RGBLED_MODE_BREATHE: + + if (_counter >= 62) + _counter = 0; + + int n; + + if (_counter < 32) { + n = _counter; + + } else { + n = 62 - _counter; + } + + _brightness = n * n / (31.0f * 31.0f); + send_led_rgb(); + break; + + case RGBLED_MODE_PATTERN: + + /* don't run out of the pattern array and stop if the next frame is 0 */ + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + _counter = 0; + + set_color(_pattern.color[_counter]); + send_led_rgb(); + _led_interval = _pattern.duration[_counter]; + break; + + default: + break; } _counter++; @@ -282,181 +318,226 @@ RGBLED::led() work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); } +/** + * Parse color constant and set _r _g _b values + */ void -RGBLED::set_color(rgbled_color_t color) { +RGBLED::set_color(rgbled_color_t color) +{ + switch (color) { + case RGBLED_COLOR_OFF: + _r = 0; + _g = 0; + _b = 0; + break; - _color = color; + case RGBLED_COLOR_RED: + _r = 255; + _g = 0; + _b = 0; + break; - switch (color) { - case RGBLED_COLOR_OFF: // off - set_rgb(0,0,0); - break; - case RGBLED_COLOR_RED: // red - set_rgb(255,0,0); - break; - case RGBLED_COLOR_YELLOW: // yellow - set_rgb(255,70,0); - break; - case RGBLED_COLOR_PURPLE: // purple - set_rgb(255,0,255); - break; - case RGBLED_COLOR_GREEN: // green - set_rgb(0,255,0); - break; - case RGBLED_COLOR_BLUE: // blue - set_rgb(0,0,255); - break; - case RGBLED_COLOR_WHITE: // white - set_rgb(255,255,255); - break; - case RGBLED_COLOR_AMBER: // amber - set_rgb(255,20,0); - break; - case RGBLED_COLOR_DIM_RED: // red - set_rgb(90,0,0); - break; - case RGBLED_COLOR_DIM_YELLOW: // yellow - set_rgb(80,30,0); - break; - case RGBLED_COLOR_DIM_PURPLE: // purple - set_rgb(45,0,45); - break; - case RGBLED_COLOR_DIM_GREEN: // green - set_rgb(0,90,0); - break; - case RGBLED_COLOR_DIM_BLUE: // blue - set_rgb(0,0,90); - break; - case RGBLED_COLOR_DIM_WHITE: // white - set_rgb(30,30,30); - break; - case RGBLED_COLOR_DIM_AMBER: // amber - set_rgb(80,20,0); - break; - default: - warnx("color unknown"); - break; + case RGBLED_COLOR_YELLOW: + _r = 255; + _g = 200; + _b = 0; + break; + + case RGBLED_COLOR_PURPLE: + _r = 255; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_GREEN: + _r = 0; + _g = 255; + _b = 0; + break; + + case RGBLED_COLOR_BLUE: + _r = 0; + _g = 0; + _b = 255; + break; + + case RGBLED_COLOR_WHITE: + _r = 255; + _g = 255; + _b = 255; + break; + + case RGBLED_COLOR_AMBER: + _r = 255; + _g = 80; + _b = 0; + break; + + case RGBLED_COLOR_DIM_RED: + _r = 90; + _g = 0; + _b = 0; + break; + + case RGBLED_COLOR_DIM_YELLOW: + _r = 80; + _g = 30; + _b = 0; + break; + + case RGBLED_COLOR_DIM_PURPLE: + _r = 45; + _g = 0; + _b = 45; + break; + + case RGBLED_COLOR_DIM_GREEN: + _r = 0; + _g = 90; + _b = 0; + break; + + case RGBLED_COLOR_DIM_BLUE: + _r = 0; + _g = 0; + _b = 90; + break; + + case RGBLED_COLOR_DIM_WHITE: + _r = 30; + _g = 30; + _b = 30; + break; + + case RGBLED_COLOR_DIM_AMBER: + _r = 80; + _g = 20; + _b = 0; + break; + + default: + warnx("color unknown"); + break; } } +/** + * Set mode, if mode not changed has no any effect (doesn't reset blinks phase) + */ void RGBLED::set_mode(rgbled_mode_t mode) { - _mode = mode; + if (mode != _mode) { + _mode = mode; - switch (mode) { + switch (mode) { case RGBLED_MODE_OFF: _should_run = false; - set_on(false); + send_led_enable(false); break; + case RGBLED_MODE_ON: - _should_run = false; - set_on(true); + _brightness = 1.0f; + send_led_rgb(); + send_led_enable(true); break; + case RGBLED_MODE_BLINK_SLOW: _should_run = true; + _counter = 0; _led_interval = 2000; + _brightness = 1.0f; + send_led_rgb(); break; + case RGBLED_MODE_BLINK_NORMAL: _should_run = true; + _counter = 0; _led_interval = 500; + _brightness = 1.0f; + send_led_rgb(); break; + case RGBLED_MODE_BLINK_FAST: _should_run = true; + _counter = 0; _led_interval = 100; + _brightness = 1.0f; + send_led_rgb(); break; + case RGBLED_MODE_BREATHE: _should_run = true; - set_on(true); _counter = 0; - _led_interval = 1000/15; + _led_interval = 25; + send_led_enable(true); break; + case RGBLED_MODE_PATTERN: _should_run = true; - set_on(true); _counter = 0; + _brightness = 1.0f; + send_led_enable(true); break; + default: warnx("mode unknown"); break; - } + } + + /* if it should run now, start the workq */ + if (_should_run && !_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); + } - /* if it should run now, start the workq */ - if (_should_run && !_running) { - _running = true; - work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); - } - /* if it should stop, then cancel the workq */ - if (!_should_run && _running) { - _running = false; - work_cancel(LPWORK, &_work); } } +/** + * Set pattern for PATTERN mode, but don't change current mode + */ void RGBLED::set_pattern(rgbled_pattern_t *pattern) { memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); - - set_mode(RGBLED_MODE_PATTERN); -} - -void -RGBLED::set_brightness(float brightness) { - - _brightness = brightness; - set_rgb(_r, _g, _b); -} - -int -RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) -{ - uint8_t settings_byte = 0; - - if (on) - settings_byte |= SETTING_ENABLE; -/* powersave not used */ -// if (not_powersave) - settings_byte |= SETTING_NOT_POWERSAVE; - - const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte}; - - return transfer(msg, sizeof(msg), nullptr, 0); } +/** + * Sent ENABLE flag to LED driver + */ int -RGBLED::set_on(bool on) +RGBLED::send_led_enable(bool enable) { uint8_t settings_byte = 0; - if (on) + if (enable) settings_byte |= SETTING_ENABLE; -/* powersave not used */ -// if (not_powersave) - settings_byte |= SETTING_NOT_POWERSAVE; + settings_byte |= SETTING_NOT_POWERSAVE; const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; return transfer(msg, sizeof(msg), nullptr, 0); } +/** + * Send RGB PWM settings to LED driver according to current color and brightness + */ int -RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) +RGBLED::send_led_rgb() { - /* save the RGB values in case we want to change the brightness later */ - _r = r; - _g = g; - _b = b; - - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; - + /* To scale from 0..255 -> 0..15 shift right by 4 bits */ + const uint8_t msg[6] = { + SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4), + SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4), + SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4) + }; return transfer(msg, sizeof(msg), nullptr, 0); } - int -RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) +RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) { uint8_t result[2]; int ret; @@ -465,24 +546,23 @@ RGBLED::get(bool &on, bool ¬_powersave, uint8_t &r, uint8_t &g, uint8_t &b) if (ret == OK) { on = result[0] & SETTING_ENABLE; - not_powersave = result[0] & SETTING_NOT_POWERSAVE; + powersave = !(result[0] & SETTING_NOT_POWERSAVE); /* XXX check, looks wrong */ - r = (result[0] & 0x0f)*255/15; - g = (result[1] & 0xf0)*255/15; - b = (result[1] & 0x0f)*255/15; + r = (result[0] & 0x0f) << 4; + g = (result[1] & 0xf0); + b = (result[1] & 0x0f) << 4; } return ret; } -void rgbled_usage(); - - -void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'"); +void +rgbled_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); - errx(0, " -a addr (0x%x)", ADDR); + warnx(" -a addr (0x%x)", ADDR); } int @@ -492,21 +572,30 @@ rgbled_main(int argc, char *argv[]) int rgbledadr = ADDR; /* 7bit */ 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); break; + case 'b': i2cdevice = strtol(optarg, NULL, 0); break; + default: rgbled_usage(); + exit(0); } } - const char *verb = argv[1]; + if (optind >= argc) { + rgbled_usage(); + exit(1); + } + + const char *verb = argv[optind]; int fd; int ret; @@ -519,17 +608,24 @@ rgbled_main(int argc, char *argv[]) // try the external bus first i2cdevice = PX4_I2C_BUS_EXPANSION; g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); + if (g_rgbled != nullptr && OK != g_rgbled->init()) { delete g_rgbled; g_rgbled = nullptr; } + 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; } } + if (g_rgbled == nullptr) { g_rgbled = new RGBLED(i2cdevice, rgbledadr); + if (g_rgbled == nullptr) errx(1, "new failed"); @@ -545,21 +641,24 @@ rgbled_main(int argc, char *argv[]) /* need the driver past this point */ if (g_rgbled == nullptr) { - warnx("not started"); - rgbled_usage(); - exit(0); + warnx("not started"); + rgbled_usage(); + exit(0); } if (!strcmp(verb, "test")) { fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF}, - {200, 200, 200, 400 } }; + rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF}, + {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern + }; ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN); close(fd); exit(ret); @@ -570,33 +669,39 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { - /* although technically it doesn't stop, this is the excepted syntax */ + if (!strcmp(verb, "off")) { fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); close(fd); exit(ret); } if (!strcmp(verb, "rgb")) { + if (argc < 5) { + errx(1, "Usage: rgbled rgb <red> <green> <blue>"); + } + fd = open(RGBLED_DEVICE_PATH, 0); + if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); } - if (argc < 5) { - errx(1, "Usage: rgbled rgb <red> <green> <blue>"); - } + rgbled_rgbset_t v; - v.red = strtol(argv[1], NULL, 0); - v.green = strtol(argv[2], NULL, 0); - v.blue = strtol(argv[3], NULL, 0); + v.red = strtol(argv[2], NULL, 0); + v.green = strtol(argv[3], NULL, 0); + v.blue = strtol(argv[4], NULL, 0); ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); + ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON); close(fd); exit(ret); } rgbled_usage(); + exit(0); } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000..d65a9be36 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -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 diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp new file mode 100644 index 000000000..e9f35cf95 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -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 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 000000000..1abecf198 --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -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 diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000..44ed03254 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -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 diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a582ece17..f36f2091e 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev("tone_alarm", "/dev/tone_alarm"), + CDev("tone_alarm", TONEALARM_DEVICE_PATH), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), @@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast + _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning _tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast + _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning } ToneAlarm::~ToneAlarm() @@ -820,10 +822,10 @@ play_tune(unsigned tune) { int fd, ret; - fd = open("/dev/tone_alarm", 0); + fd = open(TONEALARM_DEVICE_PATH, 0); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); @@ -839,10 +841,10 @@ play_string(const char *str, bool free_buffer) { int fd, ret; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = write(fd, str, strlen(str) + 1); close(fd); diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 621d3253f..3125ce246 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -67,6 +67,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #include "flow_position_control_params.h" @@ -153,20 +154,28 @@ flow_position_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow position control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[fpc] started"); uint32_t counter = 0; const float time_scale = powf(10.0f,-6.0f); /* structures */ struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_local_position_s local_pos; - + memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; + memset(&speed_sp, 0, sizeof(speed_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -216,6 +225,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -252,7 +262,7 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow position control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fpc] parameters updated."); } /* only run controller if position/speed changed */ @@ -270,6 +280,8 @@ flow_position_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); if (control_mode.flag_control_velocity_enabled) { @@ -277,6 +289,11 @@ flow_position_control_thread_main(int argc, char *argv[]) float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged"); + + status_changed = true; + /* calc dt */ if(last_time == 0) { @@ -296,7 +313,7 @@ flow_position_control_thread_main(int argc, char *argv[]) { flow_sp_sumy = filtered_flow.sumy; update_flow_sp_sumy = false; - } + } /* calc new bodyframe speed setpoints */ float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; @@ -518,6 +535,11 @@ flow_position_control_thread_main(int argc, char *argv[]) else { /* in manual or stabilized state just reset speed and flow sum setpoint */ + //mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy); + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged."); + + status_changed = false; speed_sp.vx = 0.0f; speed_sp.vy = 0.0f; flow_sp_sumx = filtered_flow.sumx; @@ -558,20 +580,20 @@ flow_position_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow position control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n"); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow position control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fpc] initialized.\n"); } } } } - printf("[flow position control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fpc] ending now...\n"); thread_running = false; diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 5e80066a7..495c415f2 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -345,6 +345,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) local_pos.y = local_pos.y + global_speed[1] * dt; local_pos.vx = global_speed[0]; local_pos.vy = global_speed[1]; + local_pos.xy_valid = true; + local_pos.v_xy_valid = true; } else { @@ -353,6 +355,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) filtered_flow.vy = 0; local_pos.vx = 0; local_pos.vy = 0; + local_pos.xy_valid = false; + local_pos.v_xy_valid = false; } /* filtering ground distance */ @@ -361,6 +365,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* not possible to fly */ sonar_valid = false; local_pos.z = 0.0f; + local_pos.z_valid = false; } else { @@ -388,6 +393,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) { local_pos.z = -sonar_new; } + + local_pos.z_valid = true; } filtered_flow.timestamp = hrt_absolute_time(); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 6af955cd7..feed0d23c 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -65,6 +65,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> #include <poll.h> +#include <mavlink/mavlink_log.h> #include "flow_speed_control_params.h" @@ -151,17 +152,23 @@ flow_speed_control_thread_main(int argc, char *argv[]) { /* welcome user */ thread_running = true; - printf("[flow speed control] starting\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd,"[fsc] started"); uint32_t counter = 0; /* structures */ struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct filtered_bottom_flow_s filtered_flow; + memset(&filtered_flow, 0, sizeof(filtered_flow)); struct vehicle_bodyframe_speed_setpoint_s speed_sp; - + memset(&speed_sp, 0, sizeof(speed_sp)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -186,6 +193,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); static bool sensors_ready = false; + static bool status_changed = false; while (!thread_should_exit) { @@ -221,7 +229,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(¶m_handles, ¶ms); - printf("[flow speed control] parameters updated.\n"); + mavlink_log_info(mavlink_fd,"[fsp] parameters updated."); } /* only run controller if position/speed changed */ @@ -237,6 +245,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); + /* get a local copy of control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); if (control_mode.flag_control_velocity_enabled) { @@ -244,6 +254,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + if(status_changed == false) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged"); + + status_changed = true; + /* limit roll and pitch corrections */ if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) { @@ -299,6 +314,11 @@ flow_speed_control_thread_main(int argc, char *argv[]) } else { + if(status_changed == true) + mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged."); + + status_changed = false; + /* reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; @@ -334,20 +354,20 @@ flow_speed_control_thread_main(int argc, char *argv[]) else if (ret == 0) { /* no return value, ignore */ - printf("[flow speed control] no attitude received.\n"); + mavlink_log_info(mavlink_fd,"[fsc] no attitude received."); } else { if (fds[0].revents & POLLIN) { sensors_ready = true; - printf("[flow speed control] initialized.\n"); + mavlink_log_info(mavlink_fd,"[fsp] initialized."); } } } } - printf("[flow speed control] ending now...\n"); + mavlink_log_info(mavlink_fd,"[fsc] ending now..."); thread_running = false; diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c new file mode 100644 index 000000000..06337be32 --- /dev/null +++ b/src/examples/hwtest/hwtest.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 hwtest.c + * + * Simple functional hardware test. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/actuator_controls.h> + +__EXPORT int ex_hwtest_main(int argc, char *argv[]); + +int ex_hwtest_main(int argc, char *argv[]) +{ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + int i; + float rcvalue = -1.0f; + hrt_abstime stime; + + while (true) { + stime = hrt_absolute_time(); + while (hrt_absolute_time() - stime < 1000000) { + for (i=0; i<8; i++) + actuators.control[i] = rcvalue; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + } + warnx("servos set to %.1f", rcvalue); + rcvalue *= -1.0f; + } + + return OK; +} diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk new file mode 100644 index 000000000..6e70863ed --- /dev/null +++ b/src/examples/hwtest/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# 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 +# 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. +# +############################################################################ + +# +# Hardware test example application +# + +MODULE_COMMAND = ex_hwtest + +SRCS = hwtest.c diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk new file mode 100644 index 000000000..f5f59a2dc --- /dev/null +++ b/src/lib/conversion/module.mk @@ -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 diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp new file mode 100644 index 000000000..b078562c2 --- /dev/null +++ b/src/lib/conversion/rotation.cpp @@ -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); + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h new file mode 100644 index 000000000..85c63c0fc --- /dev/null +++ b/src/lib/conversion/rotation.h @@ -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_ */ diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp new file mode 100644 index 000000000..d876f1a39 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -0,0 +1,150 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp + * Implementation of a simple orthogonal pitch PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_pitch_controller.h" +#include <math.h> +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_PitchController::ECL_PitchController() : + _last_run(0), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) +{ +} + +float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler, + bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = dt_micros / 1000000; + + /* lock integral for long intervals */ + if (dt_micros > 500000) + lock_integrator = true; + + float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + /* flying inverted (wings upside down) ? */ + bool inverted = false; + + /* roll is used as feedforward term and inverted flight needs to be considered */ + if (fabsf(roll) < math::radians(90.0f)) { + /* not inverted, but numerically still potentially close to infinity */ + roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f)); + } else { + /* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */ + + /* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */ + if (roll > 0.0f) { + /* right hemisphere */ + roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f)); + } else { + /* left hemisphere */ + roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f)); + } + } + + /* calculate the offset in the rate resulting from rolling */ + float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) * + tanf(roll) * sinf(roll)) * _roll_ff; + if (inverted) + turn_offset = -turn_offset; + + float pitch_error = pitch_setpoint - pitch; + /* rate setpoint from current error and time constant */ + _rate_setpoint = pitch_error / _tc; + + /* add turn offset */ + _rate_setpoint += turn_offset; + + _rate_error = _rate_setpoint - pitch_rate; + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); +} + +void ECL_PitchController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h new file mode 100644 index 000000000..1e6cec6a1 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h + * Definition of a simple orthogonal pitch PID controller. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_PITCH_CONTROLLER_H +#define ECL_PITCH_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_PitchController +{ +public: + ECL_PitchController(); + + float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f, + bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + _tc = time_constant; + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate_pos(float max_rate_pos) { + _max_rate_pos = max_rate_pos; + } + void set_max_rate_neg(float max_rate_neg) { + _max_rate_neg = max_rate_neg; + } + void set_roll_ff(float roll_ff) { + _roll_ff = roll_ff; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _rate_setpoint; + } + +private: + + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate_pos; + float _max_rate_neg; + float _roll_ff; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _max_deflection_rad; +}; + +#endif // ECL_PITCH_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp new file mode 100644 index 000000000..b9a73fc02 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp + * Implementation of a simple orthogonal roll PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "../ecl.h" +#include "ecl_roll_controller.h" +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_RollController::ECL_RollController() : + _last_run(0), + _tc(0.1f), + _last_output(0.0f), + _integrator(0.0f), + _rate_error(0.0f), + _rate_setpoint(0.0f), + _max_deflection_rad(math::radians(45.0f)) +{ + +} + +float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate, + float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + + float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); + float k_i_rate = _k_i * _tc; + + /* input conditioning */ + if (!isfinite(airspeed)) { + /* airspeed is NaN, +- INF or not available, pick center of band */ + airspeed = 0.5f * (airspeed_min + airspeed_max); + } else if (airspeed < airspeed_min) { + airspeed = airspeed_min; + } + + float roll_error = roll_setpoint - roll; + _rate_setpoint = roll_error / _tc; + + /* limit the rate */ + if (_max_rate > 0.01f) { + _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; + _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; + } + + _rate_error = _rate_setpoint - roll_rate; + + + float ilimit_scaled = 0.0f; + + if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) { + + float id = _rate_error * k_i_rate * dt * scaler; + + /* + * anti-windup: do not allow integrator to increase into the + * wrong direction if actuator is at limit + */ + if (_last_output < -_max_deflection_rad) { + /* only allow motion to center: increase value */ + id = math::max(id, 0.0f); + } else if (_last_output > _max_deflection_rad) { + /* only allow motion to center: decrease value */ + id = math::min(id, 0.0f); + } + + _integrator += id; + } + + /* integrator limit */ + _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled); + /* store non-limited output */ + _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler; + + return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad); +} + +void ECL_RollController::reset_integrator() +{ + _integrator = 0.0f; +} + diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h new file mode 100644 index 000000000..0d4ea9333 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h + * Definition of a simple orthogonal roll PID controller. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Acknowledgements: + * + * The control design is based on a design + * by Paul Riseborough and Andrew Tridgell, 2013, + * which in turn is based on initial work of + * Jonathan Challinger, 2012. + */ + +#ifndef ECL_ROLL_CONTROLLER_H +#define ECL_ROLL_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_RollController +{ +public: + ECL_RollController(); + + float control(float roll_setpoint, float roll, float roll_rate, + float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_time_constant(float time_constant) { + if (time_constant > 0.1f && time_constant < 3.0f) { + _tc = time_constant; + } + } + void set_k_p(float k_p) { + _k_p = k_p; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + void set_max_rate(float max_rate) { + _max_rate = max_rate; + } + + float get_rate_error() { + return _rate_error; + } + + float get_desired_rate() { + return _rate_setpoint; + } + +private: + uint64_t _last_run; + float _tc; + float _k_p; + float _k_i; + float _k_d; + float _integrator_max; + float _max_rate; + float _last_output; + float _integrator; + float _rate_error; + float _rate_setpoint; + float _max_deflection_rad; +}; + +#endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp new file mode 100644 index 000000000..b786acf24 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp + * Implementation of a simple orthogonal coordinated turn yaw PID controller. + * + * Authors and acknowledgements in header. + */ + +#include "ecl_yaw_controller.h" +#include <stdint.h> +#include <float.h> +#include <geo/geo.h> +#include <ecl/ecl.h> +#include <mathlib/mathlib.h> + +ECL_YawController::ECL_YawController() : + _last_run(0), + _last_error(0.0f), + _last_output(0.0f), + _last_rate_hp_out(0.0f), + _last_rate_hp_in(0.0f), + _k_d_last(0.0f), + _integrator(0.0f) +{ + +} + +float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator, + float airspeed_min, float airspeed_max, float aspeed) +{ + /* get the usual dt estimate */ + uint64_t dt_micros = ecl_elapsed_time(&_last_run); + _last_run = ecl_absolute_time(); + + float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000; + + return 0.0f; +} + +void ECL_YawController::reset_integrator() +{ + _integrator = 0.0f; +} diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h new file mode 100644 index 000000000..66b227918 --- /dev/null +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h + * Definition of a simple orthogonal coordinated turn yaw PID controller. + * + */ +#ifndef ECL_YAW_CONTROLLER_H +#define ECL_YAW_CONTROLLER_H + +#include <stdbool.h> +#include <stdint.h> + +class __EXPORT ECL_YawController +{ +public: + ECL_YawController(); + + float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false, + float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f)); + + void reset_integrator(); + + void set_k_side(float k_a) { + _k_side = k_a; + } + void set_k_i(float k_i) { + _k_i = k_i; + } + void set_k_d(float k_d) { + _k_d = k_d; + } + void set_k_roll_ff(float k_roll_ff) { + _k_roll_ff = k_roll_ff; + } + void set_integrator_max(float max) { + _integrator_max = max; + } + +private: + uint64_t _last_run; + + float _k_side; + float _k_i; + float _k_d; + float _k_roll_ff; + float _integrator_max; + + float _last_error; + float _last_output; + float _last_rate_hp_out; + float _last_rate_hp_in; + float _k_d_last; + float _integrator; + +}; + +#endif // ECL_YAW_CONTROLLER_H diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h new file mode 100644 index 000000000..e0f207696 --- /dev/null +++ b/src/lib/ecl/ecl.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h + * Adapter / shim layer for system calls needed by ECL + * + */ + +#include <drivers/drv_hrt.h> +#include <geo/geo.h> + +#define ecl_absolute_time hrt_absolute_time +#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp new file mode 100644 index 000000000..27d76f959 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -0,0 +1,366 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include "ecl_l1_pos_controller.h" + +float ECL_L1_Pos_Controller::nav_roll() +{ + float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); + ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad); + return ret; +} + +float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand() +{ + return _lateral_accel; +} + +float ECL_L1_Pos_Controller::nav_bearing() +{ + return _wrap_pi(_nav_bearing); +} + +float ECL_L1_Pos_Controller::bearing_error() +{ + return _bearing_error; +} + +float ECL_L1_Pos_Controller::target_bearing() +{ + return _target_bearing; +} + +float ECL_L1_Pos_Controller::switch_distance(float wp_radius) +{ + /* following [2], switching on L1 distance */ + return math::max(wp_radius, _L1_distance); +} + +bool ECL_L1_Pos_Controller::reached_loiter_target(void) +{ + return _circle_mode; +} + +float ECL_L1_Pos_Controller::crosstrack_error(void) +{ + return _crosstrack_error; +} + +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed_vector) +{ + + /* this follows the logic presented in [1] */ + + float eta; + float xtrack_vel; + float ltrack_vel; + + /* get the direction between the last (visited) and next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + + /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate vector from A to B */ + math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + + /* + * check if waypoints are on top of each other. If yes, + * skip A and directly continue to B + */ + if (vector_AB.length() < 1.0e-6f) { + vector_AB = get_local_planar_vector(vector_curr_position, vector_B); + } + + vector_AB.normalize(); + + /* calculate the vector from waypoint A to the aircraft */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* calculate crosstrack error (output only) */ + _crosstrack_error = vector_AB % vector_A_to_airplane; + + /* + * If the current position is in a +-135 degree angle behind waypoint A + * and further away from A than the L1 distance, then A becomes the L1 point. + * If the aircraft is already between A and B normal L1 logic is applied. + */ + float distance_A_to_airplane = vector_A_to_airplane.length(); + float alongTrackDist = vector_A_to_airplane * vector_AB; + + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); + + /* extension from [2], fly directly to A */ + if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { + + /* calculate eta to fly to waypoint A */ + + /* unit vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ + + /* calculate eta to fly to waypoint B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + + } else { + + /* calculate eta to fly along the line between A and B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ + ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ + float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ + 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, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 + float eta1 = asinf(sine_eta1); + eta = eta1 + eta2; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + + } + + /* limit angle to +-90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* flying to waypoints, not circling them */ + _circle_mode = false; + + /* the bearing angle, in NED frame */ + _bearing_error = eta; +} + +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + /* calculate the gains for the PD loop (circle tracking) */ + float omega = (2.0f * M_PI_F / _L1_period); + float K_crosstrack = omega * omega; + float K_velocity = 2.0f * _L1_damping * omega; + + /* update bearing to next waypoint */ + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + + /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length() , 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate the vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* store the normalized vector from waypoint A to current position */ + math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + + /* calculate eta angle towards the loiter center */ + + /* velocity across / orthogonal to line from waypoint to current position */ + float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; + /* velocity along line from waypoint to current position */ + float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); + float eta = atan2f(xtrack_vel_center, ltrack_vel_center); + /* limit eta to 90 degrees */ + eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); + + /* calculate the lateral acceleration to capture the center point */ + float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* for PD control: Calculate radial position and velocity errors */ + + /* radial velocity error */ + float xtrack_vel_circle = -ltrack_vel_center; + /* radial distance from the loiter circle (not center) */ + float xtrack_err_circle = vector_A_to_airplane.length() - radius; + + /* cross track error for feedback */ + _crosstrack_error = xtrack_err_circle; + + /* calculate PD update to circle waypoint */ + float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); + + /* calculate velocity on circle / along tangent */ + float tangent_vel = xtrack_vel_center * loiter_direction; + + /* prevent PD output from turning the wrong way */ + if (tangent_vel < 0.0f) { + lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f); + } + + /* calculate centripetal acceleration setpoint */ + float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle)); + + /* add PD control on circle and centripetal acceleration for total circle command */ + float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); + + /* + * Switch between circle (loiter) and capture (towards waypoint center) mode when + * the commands switch over. Only fly towards waypoint if outside the circle. + */ + + // XXX check switch over + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) | + (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { + _lateral_accel = lateral_accel_sp_center; + _circle_mode = false; + /* angle between requested and current velocity vector */ + _bearing_error = eta; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + + } else { + _lateral_accel = lateral_accel_sp_circle; + _circle_mode = true; + _bearing_error = 0.0f; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + } +} + + +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + float eta; + + /* + * As the commanded heading is the only reference + * (and no crosstrack correction occurs), + * target and navigation bearing become the same + */ + _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); + eta = _target_bearing - _wrap_pi(current_heading); + eta = _wrap_pi(eta); + + /* consequently the bearing error is exactly eta: */ + _bearing_error = eta; + + /* ground speed is the length of the ground speed vector */ + float ground_speed = ground_speed_vector.length(); + + /* adjust L1 distance to keep constant frequency */ + _L1_distance = ground_speed / _heading_omega; + float omega_vel = ground_speed * _heading_omega; + + /* not circling a waypoint */ + _circle_mode = false; + + /* navigating heading means by definition no crosstrack error */ + _crosstrack_error = 0; + + /* limit eta to 90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = 2.0f * sinf(eta) * omega_vel; +} + +void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) +{ + /* the logic in this section is trivial, but originally proposed by [2] */ + + /* reset all heading / error measures resulting in zero roll */ + _target_bearing = current_heading; + _nav_bearing = current_heading; + _bearing_error = 0; + _crosstrack_error = 0; + _lateral_accel = 0; + + /* not circling a waypoint when flying level */ + _circle_mode = false; + +} + + +math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +{ + /* this is an approximation for small angles, proposed by [2] */ + + math::Vector2f out; + + out.setX(math::radians((target.getX() - origin.getX()))); + out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + + return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH); +} + diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h new file mode 100644 index 000000000..7a3c42a92 --- /dev/null +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h + * Implementation of L1 position control. + * + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the original + * publication from [1] and does include a lot of the ideas (not code) + * from [2]. + * + * + * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle + * + */ + +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H + +#include <mathlib/mathlib.h> +#include <geo/geo.h> +#include <ecl/ecl.h> + +/** + * L1 Nonlinear Guidance Logic + */ +class __EXPORT ECL_L1_Pos_Controller +{ +public: + ECL_L1_Pos_Controller() { + _L1_period = 25; + _L1_damping = 0.75f; + } + + /** + * The current target bearing + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float nav_bearing(); + + + /** + * Get lateral acceleration demand. + * + * @return Lateral acceleration in m/s^2 + */ + float nav_lateral_acceleration_demand(); + + + /** + * Heading error. + * + * The heading error is either compared to the current track + * or to the tangent of the current loiter radius. + */ + float bearing_error(); + + + /** + * Bearing from aircraft to current target. + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float target_bearing(); + + + /** + * Get roll angle setpoint for fixed wing. + * + * @return Roll angle (in NED frame) + */ + float nav_roll(); + + + /** + * Get the current crosstrack error. + * + * @return Crosstrack error in meters. + */ + float crosstrack_error(); + + + /** + * Returns true if the loiter waypoint has been reached + */ + bool reached_loiter_target(); + + + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + + /** + * Get the switch distance + * + * This is the distance at which the system will + * switch to the next waypoint. This depends on the + * period and damping + * + * @param waypoint_switch_radius The switching radius the waypoint has set. + */ + float switch_distance(float waypoint_switch_radius); + + + /** + * Navigate between two waypoints + * + * Calling this function with two waypoints results in the + * control outputs to fly to the line segment defined by + * the points and once captured following the line segment. + * This follows the logic in [1]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, + const math::Vector2f &ground_speed); + + + /** + * Navigate on an orbit around a loiter waypoint. + * + * This allow orbits smaller than the L1 length, + * this modification was introduced in [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector2f &ground_speed_vector); + + + /** + * Navigate on a fixed bearing. + * + * This only holds a certain direction and does not perform cross + * track correction. Helpful for semi-autonomous modes. Introduced + * by [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + + + /** + * Keep the wings level. + * + * This is typically needed for maximum-lift-demand situations, + * such as takeoff or near stall. Introduced in [2]. + */ + void navigate_level_flight(float current_heading); + + + /** + * Set the L1 period. + */ + void set_l1_period(float period) { + _L1_period = period; + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate normalized frequency for heading tracking */ + _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; + } + + + /** + * Set the L1 damping factor. + * + * The original publication recommends a default of sqrt(2) / 2 = 0.707 + */ + void set_l1_damping(float damping) { + _L1_damping = damping; + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + /* calculate the L1 gain (following [2]) */ + _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 + float _L1_distance; ///< L1 lead distance, defined by period and damping + bool _circle_mode; ///< flag for loiter mode + float _nav_bearing; ///< bearing to L1 reference point + float _bearing_error; ///< bearing error + float _crosstrack_error; ///< crosstrack error in meters + float _target_bearing; ///< the heading setpoint + + float _L1_period; ///< L1 tracking period in seconds + float _L1_damping; ///< L1 damping ratio + float _L1_ratio; ///< L1 ratio for navigation + 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. + * + * This converts from latitude and longitude to planar + * coordinates with (0,0) being at the position of ref and + * returns a vector in meters towards wp. + * + * @param ref The reference position in WGS84 coordinates + * @param wp The point to convert to into the local coordinates, in WGS84 coordinates + * @return The vector in meters pointing from the reference position to the coordinates + */ + math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + +}; + + +#endif /* ECL_L1_POS_CONTROLLER_H */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk new file mode 100644 index 000000000..f2aa3db6a --- /dev/null +++ b/src/lib/ecl/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2013 Estimation and Control Library (ECL). 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. +# +############################################################################ + +# +# Estimation and Control Library +# + +SRCS = attitude_fw/ecl_pitch_controller.cpp \ + attitude_fw/ecl_roll_controller.cpp \ + attitude_fw/ecl_yaw_controller.cpp \ + l1/ecl_l1_pos_controller.cpp diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk new file mode 100644 index 000000000..53f1629e3 --- /dev/null +++ b/src/lib/external_lgpl/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# W A R N I N G: The contents of this directory are license-incompatible +# with the rest of the codebase. Do NOT copy any parts of it +# into other folders. +# +# Acknowledgements: +# +# The algorithms in this folder have been developed by Paul Riseborough +# with support from Andrew Tridgell. +# Originally licensed as LGPL for APM. As this is built as library and +# linked, use of this code does not change the usability of PX4 in general +# or any of the license implications. +# + +SRCS = tecs/tecs.cpp diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp new file mode 100644 index 000000000..864a9d24d --- /dev/null +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -0,0 +1,534 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +#include "tecs.h" +#include <ecl/ecl.h> + +using namespace math; + +#ifndef CONSTANTS_ONE_G +#define CONSTANTS_ONE_G GRAVITY +#endif + +/** + * @file tecs.cpp + * + * @author Paul Riseborough + * + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle and switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use + * of easy to measure aircraft performance data + * + */ + +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +{ + // Implement third order complementary filter for height and height rate + // estimted height rate = _integ2_state + // estimated height = _integ3_state + // Reference Paper : + // Optimising the Gains of the Baro-Inertial Vertical Channel + // Widnall W.S, Sinha P.K, + // AIAA Journal of Guidance and Control, 78-1307R + + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f; + + // printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n", + // DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2), + // accel_earth(0), accel_earth(1), accel_earth(2)); + + if (DT > 1.0f) { + _integ3_state = baro_altitude; + _integ2_state = 0.0f; + _integ1_state = 0.0f; + DT = 0.02f; // when first starting TECS, use a + // small time constant + } + + _update_50hz_last_usec = now; + _EAS = airspeed; + + // Get height acceleration + float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G); + // Perform filter calculation using backwards Euler integration + // Coefficients selected to place all three filter poles at omega + float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega; + float hgt_err = baro_altitude - _integ3_state; + float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega; + _integ1_state = _integ1_state + integ1_input * DT; + float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f; + _integ2_state = _integ2_state + integ2_input * DT; + float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f; + + // If more than 1 second has elapsed since last update then reset the integrator state + // to the measured height + if (DT > 1.0f) { + _integ3_state = baro_altitude; + + } else { + _integ3_state = _integ3_state + integ3_input * DT; + } + + // Update and average speed rate of change + // Only required if airspeed is being measured and controlled + float temp = 0; + + if (isfinite(airspeed) && airspeed_sensor_enabled()) { + // Get DCM + // Calculate speed rate of change + // XXX check + temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); + // take 5 point moving average + //_vel_dot = _vdot_filter.apply(temp); + // XXX resolve this properly + _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + + } else { + _vel_dot = 0.0f; + } + +} + +void TECS::_update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f; + _update_speed_last_usec = now; + + // Convert equivalent airspeeds to true airspeeds + + _EAS_dem = airspeed_demand; + _TAS_dem = _EAS_dem * EAS2TAS; + _TASmax = indicated_airspeed_max * EAS2TAS; + _TASmin = indicated_airspeed_min * EAS2TAS; + + // Reset states of time since last update is too large + if (DT > 1.0f) { + _integ5_state = (_EAS * EAS2TAS); + _integ4_state = 0.0f; + DT = 0.1f; // when first starting TECS, use a + // small time constant + } + + // Get airspeed or default to halfway between min and max if + // airspeed is not being used and set speed rate to zero + if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) { + // If no airspeed available use average of min and max + _EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max); + + } else { + _EAS = indicated_airspeed; + } + + // Implement a second order complementary filter to obtain a + // smoothed airspeed estimate + // airspeed estimate is held in _integ5_state + float aspdErr = (_EAS * EAS2TAS) - _integ5_state; + float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega; + + // Prevent state from winding up + if (_integ5_state < 3.1f) { + integ4_input = max(integ4_input , 0.0f); + } + + _integ4_state = _integ4_state + integ4_input * DT; + float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f; + _integ5_state = _integ5_state + integ5_input * DT; + // limit the airspeed to a minimum of 3 m/s + _integ5_state = max(_integ5_state, 3.0f); + +} + +void TECS::_update_speed_demand(void) +{ + // Set the airspeed demand to the minimum value if an underspeed condition exists + // or a bad descent condition exists + // This will minimise the rate of descent resulting from an engine failure, + // enable the maximum climb rate to be achieved and prevent continued full power descent + // into the ground due to an unachievable airspeed value + if ((_badDescent) || (_underspeed)) { + _TAS_dem = _TASmin; + } + + // Constrain speed demand + _TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax); + + // calculate velocity rate limits based on physical performance limits + // provision to use a different rate limit if bad descent or underspeed condition exists + // Use 50% of maximum energy rate to allow margin for total energy contgroller + float velRateMax; + float velRateMin; + + if ((_badDescent) || (_underspeed)) { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + + } else { + velRateMax = 0.5f * _STEdot_max / _integ5_state; + velRateMin = 0.5f * _STEdot_min / _integ5_state; + } + + // Apply rate limit + if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f; + _TAS_rate_dem = velRateMax; + + } else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) { + _TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f; + _TAS_rate_dem = velRateMin; + + } else { + _TAS_dem_adj = _TAS_dem; + _TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f; + } + + // Constrain speed demand again to protect against bad values on initialisation. + _TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax); + _TAS_dem_last = _TAS_dem; +} + +void TECS::_update_height_demand(float demand) +{ + // Apply 2 point moving average to demanded height + // This is required because height demand is only updated at 5Hz + _hgt_dem = 0.5f * (demand + _hgt_dem_in_old); + _hgt_dem_in_old = _hgt_dem; + + // printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev, + // _maxClimbRate); + + // Limit height rate of change + if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f; + + } else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) { + _hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f; + } + + _hgt_dem_prev = _hgt_dem; + + // Apply first order lag to height demand + _hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last; + _hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f; + _hgt_dem_adj_last = _hgt_dem_adj; + + // printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last, + // _hgt_rate_dem); +} + +void TECS::_detect_underspeed(void) +{ + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { + _underspeed = true; + + } else { + _underspeed = false; + } +} + +void TECS::_update_energies(void) +{ + // Calculate specific energy demands + _SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G; + _SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj; + + // Calculate specific energy rate demands + _SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G; + _SKEdot_dem = _integ5_state * _TAS_rate_dem; + + // Calculate specific energy + _SPE_est = _integ3_state * CONSTANTS_ONE_G; + _SKE_est = 0.5f * _integ5_state * _integ5_state; + + // Calculate specific energy rate + _SPEdot = _integ2_state * CONSTANTS_ONE_G; + _SKEdot = _integ5_state * _vel_dot; +} + +void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +{ + // Calculate total energy values + _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; + float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); + float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; + + // Apply 0.5 second first order filter to STEdot_error + // This is required to remove accelerometer noise from the measurement + STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast; + _STEdotErrLast = STEdot_error; + + // Calculate throttle demand + // If underspeed condition is set, then demand full throttle + if (_underspeed) { + _throttle_dem_unc = 1.0f; + + } else { + // Calculate gain scaler from specific energy error to throttle + float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + + // Calculate feed-forward throttle + float ff_throttle = 0; + float nomThr = throttle_cruise; + // Use the demanded rate of change of total energy as the feed-forward demand, but add + // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // drag increase during turns. + float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); + STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); + + if (STEdot_dem >= 0) { + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + + } else { + ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; + } + + // Calculate PD + FF throttle + _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + + // Rate limit PD + FF throttle + // Calculate the throttle increment from the specified slew time + if (fabsf(_throttle_slewrate) < 0.01f) { + float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate; + + _throttle_dem = constrain(_throttle_dem, + _last_throttle_dem - thrRateIncr, + _last_throttle_dem + thrRateIncr); + _last_throttle_dem = _throttle_dem; + } + + + // Calculate integrator state upper and lower limits + // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand + float integ_max = (_THRmaxf - _throttle_dem + 0.1f); + float integ_min = (_THRminf - _throttle_dem - 0.1f); + + // Calculate integrator state, constraining state + // Set integrator to a max throttle value dduring climbout + _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; + + if (_climbOutDem) { + _integ6_state = integ_max; + + } else { + _integ6_state = constrain(_integ6_state, integ_min, integ_max); + } + + // Sum the components. + // Only use feed-forward component if airspeed is not being used + if (airspeed_sensor_enabled()) { + _throttle_dem = _throttle_dem + _integ6_state; + + } else { + _throttle_dem = ff_throttle; + } + } + + // Constrain throttle demand + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); +} + +void TECS::_detect_bad_descent(void) +{ + // Detect a demanded airspeed too high for the aircraft to achieve. This will be + // evident by the the following conditions: + // 1) Underspeed protection not active + // 2) Specific total energy error > 200 (greater than ~20m height error) + // 3) Specific total energy reducing + // 4) throttle demand > 90% + // If these four conditions exist simultaneously, then the protection + // mode will be activated. + // Once active, the following condition are required to stay in the mode + // 1) Underspeed protection not active + // 2) Specific total energy error > 0 + // This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set + float STEdot = _SPEdot + _SKEdot; + + if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) { + _badDescent = true; + + } else { + _badDescent = false; + } +} + +void TECS::_update_pitch(void) +{ + // Calculate Speed/Height Control Weighting + // This is used to determine how the pitch control prioritises speed and height control + // A weighting of 1 provides equal priority (this is the normal mode of operation) + // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available + // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected + // or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed + // rises above the demanded value, the pitch angle will be increased by the TECS controller. + float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f); + + if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) { + SKE_weighting = 2.0f; + + } else if (!airspeed_sensor_enabled()) { + SKE_weighting = 0.0f; + } + + float SPE_weighting = 2.0f - SKE_weighting; + + // Calculate Specific Energy Balance demand, and error + float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; + float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; + float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); + float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); + + // Calculate integrator state, constraining input if pitch limits are exceeded + float integ7_input = SEB_error * _integGain; + + if (_pitch_dem_unc > _PITCHmaxf) { + integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); + + } else if (_pitch_dem_unc < _PITCHminf) { + integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc); + } + + _integ7_state = _integ7_state + integ7_input * _DT; + + // Apply max and min values for integrator state that will allow for no more than + // 5deg of saturation. This allows for some pitch variation due to gusts before the + // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence + float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G); + float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst; + _integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); + + // Calculate pitch demand from specific energy balance signals + _pitch_dem_unc = (temp + _integ7_state) / gainInv; + + // Constrain pitch demand + _pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); + + // Rate limit the pitch demand to comply with specified vertical + // acceleration limit + float ptchRateIncr = _DT * _vertAccLim / _integ5_state; + + if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { + _pitch_dem = _last_pitch_dem + ptchRateIncr; + + } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { + _pitch_dem = _last_pitch_dem - ptchRateIncr; + } + + _last_pitch_dem = _pitch_dem; +} + +void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad) +{ + // Initialise states and variables if DT > 1 second or in climbout + if (_DT > 1.0f) { + _integ6_state = 0.0f; + _integ7_state = 0.0f; + _last_throttle_dem = throttle_cruise; + _last_pitch_dem = pitch; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _hgt_dem_in_old = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + _DT = 0.1f; // when first starting TECS, use a + // small time constant + + } else if (_climbOutDem) { + _PITCHminf = ptchMinCO_rad; + _THRminf = _THRmaxf - 0.01f; + _hgt_dem_adj_last = baro_altitude; + _hgt_dem_adj = _hgt_dem_adj_last; + _hgt_dem_prev = _hgt_dem_adj_last; + _TAS_dem_last = _TAS_dem; + _TAS_dem_adj = _TAS_dem; + _underspeed = false; + _badDescent = false; + } +} + +void TECS::_update_STE_rate_lim(void) +{ + // Calculate Specific Total Energy Rate Limits + // This is a tivial calculation at the moment but will get bigger once we start adding altitude effects + _STEdot_max = _maxClimbRate * CONSTANTS_ONE_G; + _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; +} + +void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max) +{ + // Calculate time in seconds since last update + uint64_t now = ecl_absolute_time(); + _DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f; + _update_pitch_throttle_last_usec = now; + + // printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n", + // _DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max); + + // Update the speed estimate using a 2nd order complementary filter + _update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS); + + // Convert inputs + _THRmaxf = throttle_max; + _THRminf = throttle_min; + _PITCHmaxf = pitch_limit_max; + _PITCHminf = pitch_limit_min; + _climbOutDem = climbOutDem; + + // initialise selected states and variables if DT > 1 second or in climbout + _initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO); + + // Calculate Specific Total Energy Rate Limits + _update_STE_rate_lim(); + + // Calculate the speed demand + _update_speed_demand(); + + // Calculate the height demand + _update_height_demand(hgt_dem); + + // Detect underspeed condition + _detect_underspeed(); + + // Calculate specific energy quantitiues + _update_energies(); + + // Calculate throttle demand + _update_throttle(throttle_cruise, rotMat); + + // Detect bad descent due to demanded airspeed being too high + _detect_bad_descent(); + + // Calculate pitch demand + _update_pitch(); + +// // Write internal variables to the log_tuning structure. This +// // structure will be logged in dataflash at 10Hz + // log_tuning.hgt_dem = _hgt_dem_adj; + // log_tuning.hgt = _integ3_state; + // log_tuning.dhgt_dem = _hgt_rate_dem; + // log_tuning.dhgt = _integ2_state; + // log_tuning.spd_dem = _TAS_dem_adj; + // log_tuning.spd = _integ5_state; + // log_tuning.dspd = _vel_dot; + // log_tuning.ithr = _integ6_state; + // log_tuning.iptch = _integ7_state; + // log_tuning.thr = _throttle_dem; + // log_tuning.ptch = _pitch_dem; + // log_tuning.dspd_dem = _TAS_rate_dem; +} diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h new file mode 100644 index 000000000..4a98c8e97 --- /dev/null +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -0,0 +1,355 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file tecs.h +/// @brief Combined Total Energy Speed & Height Control. + +/* + * Written by Paul Riseborough 2013 to provide: + * - Combined control of speed and height using throttle to control + * total energy and pitch angle to control exchange of energy between + * potential and kinetic. + * Selectable speed or height priority modes when calculating pitch angle + * - Fallback mode when no airspeed measurement is available that + * sets throttle based on height rate demand and switches pitch angle control to + * height priority + * - Underspeed protection that demands maximum throttle switches pitch angle control + * to speed priority mode + * - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use + * of easy to measure aircraft performance data + */ + +#ifndef TECS_H +#define TECS_H + +#include <mathlib/mathlib.h> +#include <stdint.h> + +class __EXPORT TECS +{ +public: + TECS() : + + _airspeed_enabled(false), + _throttle_slewrate(0.0f), + _climbOutDem(false), + _hgt_dem_prev(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_in_old(0.0f), + _TAS_dem_last(0.0f), + _TAS_dem_adj(0.0f), + _TAS_dem(0.0f), + _integ1_state(0.0f), + _integ2_state(0.0f), + _integ3_state(0.0f), + _integ4_state(0.0f), + _integ5_state(0.0f), + _integ6_state(0.0f), + _integ7_state(0.0f), + _pitch_dem(0.0f), + _last_pitch_dem(0.0f), + _SPE_dem(0.0f), + _SKE_dem(0.0f), + _SPEdot_dem(0.0f), + _SKEdot_dem(0.0f), + _SPE_est(0.0f), + _SKE_est(0.0f), + _SPEdot(0.0f), + _SKEdot(0.0f) { + + } + + bool airspeed_sensor_enabled() { + return _airspeed_enabled; + } + + void enable_airspeed(bool enabled) { + _airspeed_enabled = enabled; + } + + // Update of the estimated height and height rate internal state + // Update of the inertial speed rate internal state + // Should be called at 50Hz or greater + void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + + // Update the control loop calculations + void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + float throttle_min, float throttle_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max); + // demanded throttle in percentage + // should return 0 to 100 + float get_throttle_demand(void) { + return _throttle_dem; + } + int32_t get_throttle_demand_percent(void) { + return get_throttle_demand(); + } + + + float get_pitch_demand() { return _pitch_dem; } + + // demanded pitch angle in centi-degrees + // should return between -9000 to +9000 + int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);} + + // 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); + + // struct PACKED log_TECS_Tuning { + // LOG_PACKET_HEADER; + // float hgt; + // float dhgt; + // float hgt_dem; + // float dhgt_dem; + // float spd_dem; + // float spd; + // float dspd; + // float ithr; + // float iptch; + // float thr; + // float ptch; + // float dspd_dem; + // } log_tuning; + + void set_time_const(float time_const) { + _timeConst = time_const; + } + + void set_min_sink_rate(float rate) { + _minSinkRate = rate; + } + + void set_max_sink_rate(float sink_rate) { + _maxSinkRate = sink_rate; + } + + void set_max_climb_rate(float climb_rate) { + _maxClimbRate = climb_rate; + } + + void set_throttle_damp(float throttle_damp) { + _thrDamp = throttle_damp; + } + + void set_integrator_gain(float gain) { + _integGain = gain; + } + + void set_vertical_accel_limit(float limit) { + _vertAccLim = limit; + } + + void set_height_comp_filter_omega(float omega) { + _hgtCompFiltOmega = omega; + } + + void set_speed_comp_filter_omega(float omega) { + _spdCompFiltOmega = omega; + } + + void set_roll_throttle_compensation(float compensation) { + _rollComp = compensation; + } + + void set_speed_weight(float weight) { + _spdWeight = weight; + } + + void set_pitch_damping(float damping) { + _ptchDamp = damping; + } + + void set_throttle_slewrate(float slewrate) { + _throttle_slewrate = slewrate; + } + + void set_indicated_airspeed_min(float airspeed) { + _indicated_airspeed_min = airspeed; + } + + void set_indicated_airspeed_max(float airspeed) { + _indicated_airspeed_max = airspeed; + } + +private: + // Last time update_50Hz was called + uint64_t _update_50hz_last_usec; + + // Last time update_speed was called + uint64_t _update_speed_last_usec; + + // Last time update_pitch_throttle was called + uint64_t _update_pitch_throttle_last_usec; + + // TECS tuning parameters + float _hgtCompFiltOmega; + float _spdCompFiltOmega; + float _maxClimbRate; + float _minSinkRate; + float _maxSinkRate; + float _timeConst; + float _ptchDamp; + float _thrDamp; + float _integGain; + float _vertAccLim; + float _rollComp; + float _spdWeight; + + // throttle demand in the range from 0.0 to 1.0 + float _throttle_dem; + + // pitch angle demand in radians + float _pitch_dem; + + // Integrator state 1 - height filter second derivative + float _integ1_state; + + // Integrator state 2 - height rate + float _integ2_state; + + // Integrator state 3 - height + float _integ3_state; + + // Integrator state 4 - airspeed filter first derivative + float _integ4_state; + + // Integrator state 5 - true airspeed + float _integ5_state; + + // Integrator state 6 - throttle integrator + float _integ6_state; + + // Integrator state 6 - pitch integrator + float _integ7_state; + + // throttle demand rate limiter state + float _last_throttle_dem; + + // pitch demand rate limiter state + float _last_pitch_dem; + + // Rate of change of speed along X axis + float _vel_dot; + + // Equivalent airspeed + float _EAS; + + // True airspeed limits + float _TASmax; + float _TASmin; + + // Current and last true airspeed demand + float _TAS_dem; + float _TAS_dem_last; + + // Equivalent airspeed demand + float _EAS_dem; + + // height demands + float _hgt_dem; + float _hgt_dem_in_old; + float _hgt_dem_adj; + float _hgt_dem_adj_last; + float _hgt_rate_dem; + float _hgt_dem_prev; + + // Speed demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_dem_adj; + + // Speed rate demand after application of rate limiting + // This is the demand tracked by the TECS control loops + float _TAS_rate_dem; + + // Total energy rate filter state + float _STEdotErrLast; + + // Underspeed condition + bool _underspeed; + + // Bad descent condition caused by unachievable airspeed demand + bool _badDescent; + + // climbout mode + bool _climbOutDem; + + // throttle demand before limiting + float _throttle_dem_unc; + + // pitch demand before limiting + float _pitch_dem_unc; + + // Maximum and minimum specific total energy rate limits + float _STEdot_max; + float _STEdot_min; + + // Maximum and minimum floating point throttle limits + float _THRmaxf; + float _THRminf; + + // Maximum and minimum floating point pitch limits + float _PITCHmaxf; + float _PITCHminf; + + // Specific energy quantities + float _SPE_dem; + float _SKE_dem; + float _SPEdot_dem; + float _SKEdot_dem; + float _SPE_est; + float _SKE_est; + float _SPEdot; + float _SKEdot; + + // Specific energy error quantities + float _STE_error; + + // Time since last update of main TECS loop (seconds) + float _DT; + + bool _airspeed_enabled; + float _throttle_slewrate; + float _indicated_airspeed_min; + float _indicated_airspeed_max; + + // Update the airspeed internal state using a second order complementary filter + void _update_speed(float airspeed_demand, float indicated_airspeed, + float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS); + + // Update the demanded airspeed + void _update_speed_demand(void); + + // Update the demanded height + void _update_height_demand(float demand); + + // Detect an underspeed condition + void _detect_underspeed(void); + + // Update Specific Energy Quantities + void _update_energies(void); + + // Update Demanded Throttle + void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + + // Detect Bad Descent + void _detect_bad_descent(void); + + // Update Demanded Pitch Angle + void _update_pitch(void); + + // Initialise states and variables + void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad); + + // Calculate specific total energy rate limits + void _update_STE_rate_lim(void); + +}; + +#endif //TECS_H diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 63792dda5..43105fdba 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -210,6 +210,36 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon); +} + +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy) +{ + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; + + double d_lat = lat_next_rad - lat_now_rad; + double d_lon = lon_next_rad - lon_now_rad; + + /* conscious mix of double and float trig function to maximize speed and efficiency */ + *vy = CONSTANTS_RADIUS_OF_EARTH * d_lon; + *vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad); +} + // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index dadec51ec..123ff80f1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -57,10 +57,6 @@ __BEGIN_DECLS #define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ #define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - // XXX remove struct crosstrack_error_s { bool past_end; // Flag indicating we are past the end of the line/arc segment @@ -116,6 +112,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy); + __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, diff --git a/src/lib/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk index fe92c8c70..f5c0647c8 100644 --- a/src/lib/mathlib/math/filter/module.mk +++ b/src/lib/mathlib/math/filter/module.mk @@ -41,4 +41,3 @@ SRCS = LowPassFilter2p.cpp # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 2146a1413..72bc7db8a 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -49,7 +49,6 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) INCLUDE_DIRS += math/arm diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee0355..799fc2fb9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam<float> _vGyro; /**< gyro process noise */ - control::BlockParam<float> _vAccel; /**< accelerometer process noise */ - control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ - control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ - control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ - control::BlockParam<float> _magDip; /**< magnetic inclination with level */ - control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam<float> _g; /**< gravitational constant */ - control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ - control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index cfa7d9e8a..5eeca5a1a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,10 +100,29 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin <anton.babushkin@me.com> */ #include "accelerometer_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include <unistd.h> @@ -112,11 +131,13 @@ #include <fcntl.h> #include <sys/prctl.h> #include <math.h> +#include <mathlib/mathlib.h> #include <string.h> #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> #include <geo/geo.h> +#include <conversion/rotation.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> @@ -127,93 +148,122 @@ #endif static const int ERROR = -1; -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +static const char *sensor_name = "accel"; + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -int do_accel_calibration(int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); +int do_accel_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - /* measure and calculate offsets & scales */ float accel_offs[3]; - float accel_scale[3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); + float accel_T[3][3]; if (res == OK) { - /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + /* measure and calculate offsets & scales */ + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + } + + if (res == OK) { + /* measurements completed successfully, rotate calibration values */ + param_t board_rotation_h = param_find("SENS_BOARD_ROT"); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; + math::Matrix board_rotation(3, 3); + get_rot_matrix(board_rotation_id, &board_rotation); + math::Matrix board_rotation_t = board_rotation.transpose(); + math::Vector3 accel_offs_vec; + accel_offs_vec.set(&accel_offs[0]); + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix accel_T_mat(3, 3); + accel_T_mat.set(&accel_T[0][0]); + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); + + /* set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + res = ERROR; } + } + if (res == OK) { + /* apply new scaling and offsets */ int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs[0], - accel_scale[0], - accel_offs[1], - accel_scale[1], - accel_offs[2], - accel_scale[2], - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (res == OK) { /* auto-save to EEPROM */ - int save_ret = param_save_default(); + res = param_save_default(); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } + } + + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - /* exit accel calibration mode */ + return res; } -int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) +{ const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; + int res = OK; while (true) { bool done = true; @@ -221,64 +271,63 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float done_count = 0; for (int i = 0; i < 6; i++) { - if (!data_collected[i]) { + if (data_collected[i]) { + done_count++; + + } else { done = false; } } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); - if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); if (done) break; + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) { - close(sensor_combined_sub); - return ERROR; + res = ERROR; + break; } if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); continue; } mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], - (double)accel_ref[orient][0], - (double)accel_ref[orient][1], - (double)accel_ref[orient][2]); + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); data_collected[orient] = true; tune_neutral(); } + close(sensor_combined_sub); - /* calculate offsets and rotation+scale matrix */ - float accel_T[3][3]; - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); - return ERROR; - } + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - /* convert accel transform matrix to scales, - * rotation part of transform matrix is not used by now - */ - for (int i = 0; i < 3; i++) { - accel_scale[i] = accel_T[i][i]; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -287,14 +336,15 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float * @return 0..5 according to orientation when vehicle is still and ready for measurements, * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ -int detect_orientation(int mavlink_fd, int sub_sensor_combined) { +int detect_orientation(int mavlink_fd, int sub_sensor_combined) +{ struct sensor_combined_s sensor; /* exponential moving average of accel */ float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ @@ -318,30 +368,38 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { while (true) { /* wait blocking for new data */ int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); t = hrt_absolute_time(); float dt = (t - t_prev) / 1000000.0f; t_prev = t; float w = dt / ema_len; + for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); + + if (d > still_thr2 * 8.0f) + d = still_thr2 * 8.0f; + if (d > accel_disp[i]) accel_disp[i] = d; } + /* still detector with hysteresis */ - if ( accel_disp[0] < still_thr2 && - accel_disp[1] < still_thr2 && - accel_disp[2] < still_thr2 ) { + if (accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2) { /* is still now */ if (t_still == 0) { /* first time */ mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; + } else { /* still since t_still */ if (t > t_still + still_time) { @@ -349,62 +407,71 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else if ( accel_disp[0] > still_thr2 * 2.0f || - accel_disp[1] > still_thr2 * 2.0f || - accel_disp[2] > still_thr2 * 2.0f) { + + } else if (accel_disp[0] > still_thr2 * 4.0f || + accel_disp[1] > still_thr2 * 4.0f || + accel_disp[2] > still_thr2 * 4.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); + mavlink_log_info(mavlink_fd, "detected motion, hold still..."); t_still = 0; } } + } else if (poll_ret == 0) { poll_errcount++; } + if (t > t_timeout) { poll_errcount++; } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + return ERROR; } } - if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 0; // [ g, 0, 0 ] - if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 1; // [ -g, 0, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 2; // [ 0, g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr) return 3; // [ 0, -g, 0 ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) return 4; // [ 0, 0, g ] - if ( fabsf(accel_ema[0]) < accel_err_thr && - fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + + if (fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) +{ struct pollfd fds[1]; fds[0].fd = sensor_combined_sub; fds[0].events = POLLIN; @@ -415,12 +482,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) accel_sum[i] += sensor.accelerometer_m_s2[i]; + count++; + } else { errcount++; continue; @@ -437,10 +508,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp return OK; } -int mat_invert3(float src[3][3], float dst[3][3]) { +int mat_invert3(float src[3][3], float dst[3][3]) +{ float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0f) return ERROR; // Singular matrix @@ -457,7 +530,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) { return OK; } -int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) +{ /* calculate offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; @@ -466,6 +540,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { float a = accel_ref[i * 2][j] - accel_offs[j]; @@ -475,6 +550,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; + if (mat_invert3(mat_A, mat_A_inv) != OK) return ERROR; diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h new file mode 100644 index 000000000..fd8b8564d --- /dev/null +++ b/src/modules/commander/calibration_messages.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * 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 calibration_messages.h + * + * Common calibration messages. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef CALIBRATION_MESSAGES_H_ +#define CALIBRATION_MESSAGES_H_ + +#define CAL_STARTED_MSG "%s calibration: started" +#define CAL_DONE_MSG "%s calibration: done" +#define CAL_FAILED_MSG "%s calibration: failed" +#define CAL_PROGRESS_MSG "%s calibration: progress <%u>" + +#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor" +#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration" +#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration" +#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters" +#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters" + +#endif /* CALIBRATION_MESSAGES_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 333fe30ae..44a144296 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -5,6 +5,7 @@ * Lorenz Meier <lm@inf.ethz.ch> * Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -144,8 +145,8 @@ static int mavlink_fd; /* flags */ static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static unsigned int leds_counter; @@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -230,7 +231,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("commander already running\n"); + warnx("commander already running"); /* this is not an error */ exit(0); } @@ -242,21 +243,38 @@ int commander_main(int argc, char *argv[]) 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + if (!thread_running) + errx(0, "commander already stopped"); + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("\tcommander is running\n"); + warnx("\tcommander is running"); print_status(); } else { - warnx("\tcommander not started\n"); + warnx("\tcommander not started"); } exit(0); @@ -326,6 +344,9 @@ void print_status() warnx("arming: %s", armed_str); } +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -338,14 +359,38 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -354,7 +399,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -438,9 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - // XXX implement later - result = VEHICLE_CMD_RESULT_DENIED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + } break; default: @@ -457,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe tune_negative(); if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); } } @@ -483,6 +547,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -526,7 +593,6 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value @@ -536,10 +602,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -595,16 +657,20 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] started"); + int ret; + pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); /* Start monitoring loop */ unsigned counter = 0; @@ -617,7 +683,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; - uint64_t start_time = 0; bool status_changed = true; @@ -625,7 +690,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check()); + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -695,7 +760,7 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - toggle_status_leds(&status, &armed, true); + control_status_leds(&status, &armed, true); /* now initialized */ commander_initialized = true; @@ -740,7 +805,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check()); + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); @@ -773,16 +838,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); - orb_check(cmd_sub, &updated); - - if (updated) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - /* update safety topic */ orb_check(safety_sub, &updated); @@ -819,10 +874,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } } @@ -835,8 +890,8 @@ int commander_thread_main(int argc, char *argv[]) // warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -888,7 +943,7 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - struct stat statbuf; + //struct stat statbuf; //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } @@ -900,7 +955,7 @@ int commander_thread_main(int argc, char *argv[]) //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; @@ -912,16 +967,15 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { - // XXX not sure what should happen when voltage is low in flight - //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + } else { - // XXX should we still allow to arm with critical battery? - //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -930,6 +984,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -939,7 +994,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1014,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; } } @@ -1043,7 +1098,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1056,10 +1111,18 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); + } + stick_on_counter = 0; } else { @@ -1079,15 +1142,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates safety switch not pressed */ - - if (!(!safety.safety_switch_available || safety.safety_off)) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1103,25 +1159,37 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } } } + + /* handle commands last, as the system needs to be updated to handle them */ + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } /* check which state machines for changes, clear "changed" flag */ @@ -1129,34 +1197,19 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - status_changed = true; - - } else { - status_changed = false; - } - hrt_abstime t1 = hrt_absolute_time(); - /* publish arming state */ - if (arming_state_changed) { - armed.timestamp = t1; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); - } - - /* publish control mode */ if (navigation_state_changed || arming_state_changed) { - /* publish new navigation state */ control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - control_mode.counter++; - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { - status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); control_mode.timestamp = t1; @@ -1194,13 +1247,33 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; - toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + int blink_state = blink_msg_state(); + + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; usleep(COMMANDER_MONITORING_INTERVAL); } /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); + ret = pthread_join(commander_low_prio_thread, NULL); + + if (ret) { + warn("join failed: %d", ret); + } + + rgbled_set_mode(RGBLED_MODE_OFF); /* close fds */ led_deinit(); @@ -1218,9 +1291,6 @@ int commander_thread_main(int argc, char *argv[]) close(param_changed_sub); close(battery_sub); - warnx("exiting"); - fflush(stdout); - thread_running = false; return 0; @@ -1239,8 +1309,50 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + if (set_normal_color) { + /* set color */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + rgbled_set_color(RGBLED_COLOR_AMBER); + } + + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ @@ -1261,46 +1373,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif - if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ - - if (armed->armed) { - rgbled_set_mode(RGBLED_MODE_ON); - } else if (armed->ready_to_arm) { - rgbled_set_mode(RGBLED_MODE_BREATHE); - } else { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - } - } - - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - default: - break; - } - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - default: - break; - } - } - /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { if (leds_counter % 2 == 0) @@ -1434,7 +1506,7 @@ print_reject_mode(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] WARNING: reject %s", msg); + sprintf(s, "#audio: warning: reject %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1448,7 +1520,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] %s", msg); + sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1468,16 +1540,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c return TRANSITION_NOT_CHANGED; } } + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ if (local_pos->z > -takeoff_alt || status->condition_landed) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } } + /* switch to AUTO mode */ if (status->rc_signal_found_once && !status->rc_signal_lost) { /* act depending on switches when manual control enabled */ @@ -1495,18 +1569,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } + } else { /* switch to MISSION when no RC control and first time in some AUTO mode */ if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { res = TRANSITION_NOT_CHANGED; } else { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1541,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (res == TRANSITION_CHANGED) { if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); } else { if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); } } } @@ -1584,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); tune_negative(); break; @@ -1626,11 +1702,10 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { + /* wait for up to 200ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) continue; @@ -1684,7 +1759,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1705,7 +1780,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { @@ -1724,7 +1799,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } @@ -1733,35 +1808,41 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } @@ -1780,10 +1861,9 @@ void *commander_low_prio_loop(void *arg) /* send acknowledge command */ // XXX TODO } - } close(cmd_sub); - return 0; + return NULL; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 7feace2b4..565b4b66a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +56,6 @@ #include <drivers/drv_led.h> #include <drivers/drv_rgbled.h> - #include "commander_helper.h" /* oddly, ERROR is not defined for c++ */ @@ -64,25 +64,28 @@ #endif static const int ERROR = -1; +#define BLINK_MSG_TIME 700000 // 3 fast blinks + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } static int buzzer; +static hrt_abstime blink_msg_end; int buzzer_init() { - buzzer = open("/dev/tone_alarm", O_WRONLY); + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { warnx("Buzzer: open fail\n"); @@ -104,16 +107,25 @@ void tune_error() void tune_positive() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_WHITE); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } @@ -132,18 +144,31 @@ int tune_critical_bat() return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } - - void tune_stop() { ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + static int leds; static int rgbleds; int led_init() { + blink_msg_end = 0; + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); @@ -159,6 +184,7 @@ int led_init() warnx("Blue LED: ioctl fail\n"); return ERROR; } + #endif if (ioctl(leds, LED_ON, LED_AMBER)) { @@ -168,6 +194,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 errx(1, "Unable to open " RGBLED_DEVICE_PATH); @@ -203,19 +230,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } -void rgbled_set_mode(rgbled_mode_t mode) { +void rgbled_set_mode(rgbled_mode_t mode) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } -void rgbled_set_pattern(rgbled_pattern_t *pattern) { +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 027d0535f..e9514446c 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,7 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); +int blink_msg_state(); int led_init(void); void led_deinit(void); @@ -70,9 +71,7 @@ int led_on(int led); int led_off(int led); void rgbled_set_color(rgbled_color_t color); - void rgbled_set_mode(rgbled_mode_t mode); - void rgbled_set_pattern(rgbled_pattern_t *pattern); /** diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 82a0349c9..30cd0d48d 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -33,10 +33,12 @@ /** * @file gyro_calibration.cpp + * * Gyroscope calibration routine */ #include "gyro_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include <stdio.h> @@ -56,21 +58,14 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "gyro"; + int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); - - const unsigned calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); - unsigned calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { + struct gyro_scale gyro_scale = { 0.0f, 1.0f, 0.0f, @@ -79,95 +74,100 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); + int res = OK; + /* reset all offsets to zero and all scales to one */ + int fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); - unsigned poll_errcount = 0; - - while (calibration_counter < calibration_count) { + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_combined; - fds[0].events = POLLIN; + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; + + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; + + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } + } - int poll_ret = poll(fds, 1, 1000); + close(sub_sensor_gyro); - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); - - } else { - poll_errcount++; - } + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; + } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_combined); - return ERROR; + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; } } - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params"); + res = ERROR; } + } - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); +#if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); + tune_neutral(); - close(fd); + /* scale calibration */ + /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_combined); - return ERROR; - } + mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); - tune_neutral(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - close(sub_sensor_combined); - return ERROR; - } + /* apply new offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + warn("WARNING: failed to apply new offsets for gyro"); - /*** --- SCALING --- ***/ + close(fd); - mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); - warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); unsigned rotations_count = 30; float gyro_integral = 0.0f; @@ -176,9 +176,11 @@ int do_gyro_calibration(int mavlink_fd) // XXX change to mag topic orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + + if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; uint64_t last_time = hrt_absolute_time(); @@ -188,7 +190,7 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { + && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; @@ -216,14 +218,17 @@ int do_gyro_calibration(int mavlink_fd) // calculate error between estimate and measurement // apply declination correction for true heading as well. //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; + float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag > M_PI_F) mag -= 2 * M_PI_F; + + if (mag < -M_PI_F) mag += 2 * M_PI_F; float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; + if (diff > M_PI_F) diff -= 2 * M_PI_F; + + if (diff < -M_PI_F) diff += 2 * M_PI_F; baseline_integral += diff; mag_last = mag; @@ -233,61 +238,68 @@ int do_gyro_calibration(int mavlink_fd) // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; } } float gyro_scale = baseline_integral / gyro_integral; - float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); - if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); + close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); + return ERROR; + } + + /* beep on calibration end */ + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); + tune_neutral(); - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); +#endif + + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); + res = ERROR; } + } - /* set offsets to actual value */ + if (res == OK) { + /* apply new scaling and offsets */ fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - gyro_scales[0], - gyro_offset[1], - gyro_scales[1], - gyro_offset[2], - gyro_scales[2], - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } + } + + if (res == OK) { /* auto-save to EEPROM */ - int save_ret = param_save_default(); + res = param_save_default(); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } + } - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - /* third beep by cal end routine */ - close(sub_sensor_combined); - return OK; } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - close(sub_sensor_combined); - return ERROR; + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } + + return res; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b0d4266be..09f4104f8 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -33,12 +33,14 @@ /** * @file mag_calibration.cpp + * * Magnetometer calibration routine */ #include "mag_calibration.h" #include "commander_helper.h" #include "calibration_routines.h" +#include "calibration_messages.h" #include <stdio.h> #include <stdlib.h> @@ -59,26 +61,20 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "mag"; + int do_mag_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); /* 45 seconds */ uint64_t calibration_interval = 45 * 1000 * 1000; - /* maximum 2000 values */ + /* maximum 500 values */ const unsigned int calibration_maxcount = 500; unsigned int calibration_counter = 0; - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ struct mag_scale mscale_null = { 0.0f, 1.0f, @@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd) 1.0f, }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } + int res = OK; - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } + /* erase old calibration */ + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); - close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + } - mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + if (res == OK) { + /* calibrate range */ + res = ioctl(fd, MAGIOCCALIBRATE, fd); - /* calibrate offsets */ + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + } + } - // uint64_t calibration_start = hrt_absolute_time(); + close(fd); - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + float *x; + float *y; + float *z; - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; + if (res == OK) { + /* allocate memory */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = (float *)malloc(sizeof(float) * calibration_maxcount); + y = (float *)malloc(sizeof(float) * calibration_maxcount); + z = (float *)malloc(sizeof(float) * calibration_maxcount); - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return ERROR; + if (x == NULL || y == NULL || z == NULL) { + mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + res = ERROR; + } } - mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + if (res == OK) { + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; - unsigned poll_errcount = 0; + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { + /* calibrate offsets */ + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + unsigned poll_errcount = 0; - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; + mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { - axis_index++; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; - mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); - tune_neutral(); + int poll_ret = poll(fds, 1, 1000); - axis_deadline += calibration_interval / 3; - } + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - if (!(axis_index < 3)) { - break; - } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; + calibration_counter++; - calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) - mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount); + if (calibration_counter % (calibration_maxcount / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } else { + poll_errcount++; + } - } else { - poll_errcount++; - } - - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); - close(sub_mag); - free(x); - free(y); - free(z); - return ERROR; + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); + res = ERROR; + break; + } } - + close(sub_mag); } float sphere_x; @@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd) float sphere_z; float sphere_radius; - mavlink_log_info(mavlink_fd, "mag cal progress <70> percent"); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_log_info(mavlink_fd, "mag cal progress <80> percent"); + if (res == OK) { + /* sphere fit */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - free(x); - free(y); - free(z); + if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { + mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); + res = ERROR; + } + } - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + if (x != NULL) + free(x); - fd = open(MAG_DEVICE_PATH, 0); + if (y != NULL) + free(y); - struct mag_scale mscale; + if (z != NULL) + free(z); - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); + if (res == OK) { + /* apply calibration and set parameters */ + struct mag_scale mscale; - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; + fd = open(MAG_DEVICE_PATH, 0); + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); + } - close(fd); + if (res == OK) { + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; - /* announce and set new offset */ + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); + } } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } + close(fd); - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } + if (res == OK) { + /* set parameters */ + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + res = ERROR; - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + res = ERROR; + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + res = ERROR; - mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + res = ERROR; - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + } - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - close(sub_mag); - return ERROR; + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); + if (res != OK) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); + } + } - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); - mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + if (res == OK) { + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - close(sub_mag); - return OK; - /* third beep by cal end routine */ + } else { + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + } - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - close(sub_mag); - return ERROR; + return res; } } diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 91d75121e..554dfcb08 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,4 +47,3 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp - diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cef10185..490fc8fc6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -228,8 +238,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need altitude estimate */ - if (current_state->condition_local_altitude_valid) { + /* need at minimum altitude estimate */ + if (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -237,8 +248,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need local position estimate */ - if (current_state->condition_local_position_valid) { + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -464,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); @@ -495,13 +504,14 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (valid_transition) { current_status->hil_state = new_state; - current_status->counter++; current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); current_control_mode->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + // XXX also set lockdown here + ret = OK; } else { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d..36bc8c24b 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template<class T> -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase +{ +public: + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + float get() { return _val; } + void set(float val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + int get() { return _val; } + void set(int val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702..66e929038 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam<float> _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam<float> _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam<float> _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam<float> _kP; - BlockParam<float> _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam<float> _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam<float> _mean; - BlockParam<float> _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp new file mode 100644 index 000000000..eb67874db --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -0,0 +1,778 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_att_control_main.c + * Implementation of a generic attitude controller based on classic orthogonal PIDs. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +#include <ecl/attitude_fw/ecl_pitch_controller.h> +#include <ecl/attitude_fw/ecl_roll_controller.h> +#include <ecl/attitude_fw/ecl_yaw_controller.h> + +/** + * Fixedwing attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]); + +class FixedwingAttitudeControl +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vcontrol_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_roll_feedforward; + float y_integrator_max; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t tconst; + param_t p_p; + param_t p_d; + param_t p_i; + param_t p_rmax_pos; + param_t p_rmax_neg; + param_t p_integrator_max; + param_t p_roll_feedforward; + param_t r_p; + param_t r_d; + param_t r_i; + param_t r_integrator_max; + param_t r_rmax; + param_t y_p; + param_t y_i; + param_t y_d; + param_t y_roll_feedforward; + param_t y_integrator_max; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingAttitudeControl *g_control; +} + +FixedwingAttitudeControl::FixedwingAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vcontrol_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + _attitude_sp_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_valid(false) +{ + _parameter_handles.tconst = param_find("FW_ATT_TC"); + _parameter_handles.p_p = param_find("FW_P_P"); + _parameter_handles.p_d = param_find("FW_P_D"); + _parameter_handles.p_i = param_find("FW_P_I"); + _parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS"); + _parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG"); + _parameter_handles.p_integrator_max = param_find("FW_P_integrator_max"); + _parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF"); + + _parameter_handles.r_p = param_find("FW_R_P"); + _parameter_handles.r_d = param_find("FW_R_D"); + _parameter_handles.r_i = param_find("FW_R_I"); + _parameter_handles.r_integrator_max = param_find("FW_R_integrator_max"); + _parameter_handles.r_rmax = param_find("FW_R_RMAX"); + + _parameter_handles.y_p = param_find("FW_Y_P"); + _parameter_handles.y_i = param_find("FW_Y_I"); + _parameter_handles.y_d = param_find("FW_Y_D"); + _parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF"); + _parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingAttitudeControl::~FixedwingAttitudeControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +FixedwingAttitudeControl::parameters_update() +{ + + param_get(_parameter_handles.tconst, &(_parameters.tconst)); + param_get(_parameter_handles.p_p, &(_parameters.p_p)); + param_get(_parameter_handles.p_d, &(_parameters.p_d)); + param_get(_parameter_handles.p_i, &(_parameters.p_i)); + param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos)); + param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg)); + param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max)); + param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward)); + + param_get(_parameter_handles.r_p, &(_parameters.r_p)); + param_get(_parameter_handles.r_d, &(_parameters.r_d)); + param_get(_parameter_handles.r_i, &(_parameters.r_i)); + param_get(_parameter_handles.r_integrator_max, &(_parameters.r_integrator_max)); + param_get(_parameter_handles.r_rmax, &(_parameters.r_rmax)); + + param_get(_parameter_handles.y_p, &(_parameters.y_p)); + param_get(_parameter_handles.y_i, &(_parameters.y_i)); + param_get(_parameter_handles.y_d, &(_parameters.y_d)); + param_get(_parameter_handles.y_roll_feedforward, &(_parameters.y_roll_feedforward)); + param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + /* pitch control parameters */ + _pitch_ctrl.set_time_constant(_parameters.tconst); + _pitch_ctrl.set_k_p(math::radians(_parameters.p_p)); + _pitch_ctrl.set_k_i(math::radians(_parameters.p_i)); + _pitch_ctrl.set_k_d(math::radians(_parameters.p_d)); + _pitch_ctrl.set_integrator_max(math::radians(_parameters.p_integrator_max)); + _pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos)); + _pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg)); + _pitch_ctrl.set_roll_ff(math::radians(_parameters.p_roll_feedforward)); + + /* roll control parameters */ + _roll_ctrl.set_time_constant(_parameters.tconst); + _roll_ctrl.set_k_p(math::radians(_parameters.r_p)); + _roll_ctrl.set_k_i(math::radians(_parameters.r_i)); + _roll_ctrl.set_k_d(math::radians(_parameters.r_d)); + _roll_ctrl.set_integrator_max(math::radians(_parameters.r_integrator_max)); + _roll_ctrl.set_max_rate(math::radians(_parameters.r_rmax)); + + /* yaw control parameters */ + _yaw_ctrl.set_k_side(math::radians(_parameters.y_p)); + _yaw_ctrl.set_k_i(math::radians(_parameters.y_i)); + _yaw_ctrl.set_k_d(math::radians(_parameters.y_d)); + _yaw_ctrl.set_k_roll_ff(math::radians(_parameters.y_roll_feedforward)); + _yaw_ctrl.set_integrator_max(math::radians(_parameters.y_integrator_max)); + + return OK; +} + +void +FixedwingAttitudeControl::vehicle_control_mode_poll() +{ + bool vcontrol_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); + + if (vcontrol_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); + } +} + +void +FixedwingAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +bool +FixedwingAttitudeControl::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +void +FixedwingAttitudeControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +FixedwingAttitudeControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vcontrol_mode_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + vehicle_control_mode_poll(); + + vehicle_manual_poll(); + + /* lock integrator until control is started */ + bool lock_integrator; + + if (_vcontrol_mode.flag_control_attitude_enabled) { + lock_integrator = false; + + } else { + lock_integrator = true; + } + + /* decide if in stabilized or full manual control */ + + if (_vcontrol_mode.flag_control_attitude_enabled) { + + /* scale from radians to normalized -1 .. 1 range */ + const float actuator_scaling = 1.0f / (M_PI_F / 4.0f); + + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is smaller than min, the sensor is not giving good readings */ + if (!_airspeed_valid || + (_airspeed.indicated_airspeed_m_s < _parameters.airspeed_min) || + !isfinite(_airspeed.indicated_airspeed_m_s)) { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + + } else { + airspeed = _airspeed.indicated_airspeed_m_s; + } + + float airspeed_scaling = _parameters.airspeed_trim / airspeed; + //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + throttle_sp = _manual.throttle; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f; + att_sp.thrust = throttle_sp; + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } + } + + float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed, + airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f; + + float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling, + lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f; + + float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator, + _parameters.airspeed_min, _parameters.airspeed_max, airspeed); + _actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f; + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + + // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", + // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], + // _actuators.control[2], _actuators.control[3]); + + /* + * Lazily publish the rate setpoint (for analysis, the actuators are published below) + * only once available + */ + vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = _roll_ctrl.get_desired_rate(); + rates_sp.pitch = _pitch_ctrl.get_desired_rate(); + rates_sp.yaw = 0.0f; // XXX not yet implemented + + rates_sp.timestamp = hrt_absolute_time(); + + if (_rate_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + + } else { + /* manual/direct control */ + _actuators.control[0] = _manual.roll; + _actuators.control[1] = _manual.pitch; + _actuators.control[2] = _manual.yaw; + _actuators.control[3] = _manual.throttle; + _actuators.control[4] = _manual.flaps; + } + + _actuators.control[5] = _manual.aux1; + _actuators.control[6] = _manual.aux2; + _actuators.control[7] = _manual.aux3; + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new FixedwingAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c new file mode 100644 index 000000000..97aa275de --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ + +// @DisplayName Attitude Time Constant +// @Description This defines the latency between a step input and the achieved setpoint. Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed. +// @Range 0.4 to 1.0 seconds, in tens of seconds +PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); + +// @DisplayName Proportional gain. +// @Description This defines how much the elevator input will be commanded dependend on the current pitch error. +// @Range 10 to 200, 1 increments +PARAM_DEFINE_FLOAT(FW_P_P, 40.0f); + +// @DisplayName Damping gain. +// @Description This gain damps the airframe pitch rate. In particular relevant for flying wings. +// @Range 0.0 to 10.0, 0.1 increments +PARAM_DEFINE_FLOAT(FW_P_D, 0.0f); + +// @DisplayName Integrator gain. +// @Description This gain defines how much control response will result out of a steady state error. It trims any constant error. +// @Range 0 to 50.0 +PARAM_DEFINE_FLOAT(FW_P_I, 0.0f); + +// @DisplayName Maximum positive / up pitch rate. +// @Description This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f); + +// @DisplayName Maximum negative / down pitch rate. +// @Description This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds, in 1 increments +PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f); + +// @DisplayName Pitch Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_P_IMAX, 15.0f); + +// @DisplayName Roll feedforward gain. +// @Description This compensates during turns and ensures the nose stays level. +// @Range 0.5 2.0 +// @Increment 0.05 +// @User User +PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 1.0f); + +// @DisplayName Proportional Gain. +// @Description This gain controls the roll angle to roll actuator output. +// @Range 10.0 200.0 +// @Increment 10.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_P, 40.0f); + +// @DisplayName Damping Gain +// @Description Controls the roll rate to roll actuator output. It helps to reduce motions in turbulence. +// @Range 0.0 10.0 +// @Increment 1.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_D, 0.0f); + +// @DisplayName Integrator Gain +// @Description This gain controls the contribution of the integral to roll actuator outputs. It trims out steady state errors. +// @Range 0.0 100.0 +// @Increment 5.0 +// @User User +PARAM_DEFINE_FLOAT(FW_R_I, 0.0f); + +// @DisplayName Roll Integrator Anti-Windup +// @Description This limits the range in degrees the integrator can wind up to. +// @Range 0.0 to 45.0 +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_IMAX, 15.0f); + +// @DisplayName Maximum Roll Rate +// @Description This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit. +// @Range 0 to 90.0 degrees per seconds +// @Increment 1.0 +PARAM_DEFINE_FLOAT(FW_R_RMAX, 60); + + +PARAM_DEFINE_FLOAT(FW_Y_P, 0); +PARAM_DEFINE_FLOAT(FW_Y_I, 0); +PARAM_DEFINE_FLOAT(FW_Y_IMAX, 15.0f); +PARAM_DEFINE_FLOAT(FW_Y_D, 0); +PARAM_DEFINE_FLOAT(FW_Y_ROLLFF, 1); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 9.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 12.0f); +PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 18.0f); diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk new file mode 100644 index 000000000..1e600757e --- /dev/null +++ b/src/modules/fw_att_control/module.mk @@ -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. +# +############################################################################ + +# +# Fixedwing attitude control application +# + +MODULE_COMMAND = fw_att_control + +SRCS = fw_att_control_main.cpp \ + fw_att_control_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp new file mode 100644 index 000000000..ffa7915a7 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -0,0 +1,1218 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 fw_pos_control_l1_main.c + * Implementation of a generic position controller based on the L1 norm. Outputs a bank / roll + * angle, equivalent to a lateral motion (for copters and rovers). + * + * Original publication for horizontal control class: + * S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * Original implementation for total energy control class: + * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * + * More details and acknowledgements in the referenced library headers. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/navigation_capabilities.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +#include <ecl/l1/ecl_l1_pos_controller.h> +#include <external_lgpl/tecs/tecs.h> + +/** + * L1 control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); + +class FixedwingPositionControl +{ +public: + /** + * Constructor + */ + FixedwingPositionControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingPositionControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _global_set_triplet_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _control_mode_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _accel_sub; /**< body frame accelerations */ + + orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + struct accel_report _accel; /**< body frame accelerations */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + float _launch_lat; + float _launch_lon; + float _launch_alt; + bool _launch_valid; + + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + + /* throttle and airspeed states */ + float _airspeed_error; ///< airspeed error to setpoint in m/s + bool _airspeed_valid; ///< flag if a valid airspeed estimate exists + uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures + float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s + bool _global_pos_valid; ///< global position is valid + math::Dcm _R_nb; ///< current attitude + + ECL_L1_Pos_Controller _l1_control; + TECS _tecs; + + struct { + float l1_period; + float l1_damping; + + float time_const; + float min_sink_rate; + float max_sink_rate; + float max_climb_rate; + float throttle_damp; + float integrator_gain; + float vertical_accel_limit; + float height_comp_filter_omega; + float speed_comp_filter_omega; + float roll_throttle_compensation; + float speed_weight; + float pitch_damping; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float pitch_limit_min; + float pitch_limit_max; + float roll_limit; + float throttle_min; + float throttle_max; + float throttle_cruise; + + float throttle_land_max; + + float loiter_hold_radius; + } _parameters; /**< local copies of interesting parameters */ + + struct { + + param_t l1_period; + param_t l1_damping; + + param_t time_const; + param_t min_sink_rate; + param_t max_sink_rate; + param_t max_climb_rate; + param_t throttle_damp; + param_t integrator_gain; + param_t vertical_accel_limit; + param_t height_comp_filter_omega; + param_t speed_comp_filter_omega; + param_t roll_throttle_compensation; + param_t speed_weight; + param_t pitch_damping; + + param_t airspeed_min; + param_t airspeed_trim; + param_t airspeed_max; + + param_t pitch_limit_min; + param_t pitch_limit_max; + param_t roll_limit; + param_t throttle_min; + param_t throttle_max; + param_t throttle_cruise; + + param_t throttle_land_max; + + param_t loiter_hold_radius; + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_control_mode_poll(); + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Control position. + */ + bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet); + + float calculate_target_airspeed(float airspeed_demand); + void calculate_gndspeed_undershoot(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace l1_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingPositionControl *g_control; +} + +FixedwingPositionControl::FixedwingPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _global_set_triplet_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), +/* states */ + _setpoint_valid(false), + _loiter_hold(false), + _airspeed_error(0.0f), + _airspeed_valid(false), + _groundspeed_undershoot(0.0f), + _global_pos_valid(false), + land_noreturn(false) +{ + _nav_capabilities.turn_distance = 0.0f; + + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); + _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); + _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); + + _parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN"); + _parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX"); + + _parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN"); + _parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX"); + _parameter_handles.roll_limit = param_find("FW_R_LIM"); + _parameter_handles.throttle_min = param_find("FW_THR_MIN"); + _parameter_handles.throttle_max = param_find("FW_THR_MAX"); + _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); + _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); + _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); + _parameter_handles.max_climb_rate = param_find("FW_T_CLMB_MAX"); + _parameter_handles.throttle_damp = param_find("FW_T_THR_DAMP"); + _parameter_handles.integrator_gain = param_find("FW_T_INTEG_GAIN"); + _parameter_handles.vertical_accel_limit = param_find("FW_T_VERT_ACC"); + _parameter_handles.height_comp_filter_omega = param_find("FW_T_HGT_OMEGA"); + _parameter_handles.speed_comp_filter_omega = param_find("FW_T_SPD_OMEGA"); + _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); + _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); + _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingPositionControl::~FixedwingPositionControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + l1_control::g_control = nullptr; +} + +int +FixedwingPositionControl::parameters_update() +{ + + /* L1 control parameters */ + param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping)); + param_get(_parameter_handles.l1_period, &(_parameters.l1_period)); + param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius)); + + param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min)); + param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim)); + param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max)); + + param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min)); + param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max)); + param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit)); + param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min)); + param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); + param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + + param_get(_parameter_handles.time_const, &(_parameters.time_const)); + param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); + param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); + param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp)); + param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain)); + param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit)); + param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega)); + param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega)); + param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation)); + param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight)); + param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); + param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + _l1_control.set_l1_damping(_parameters.l1_damping); + _l1_control.set_l1_period(_parameters.l1_period); + _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); + + _tecs.set_time_const(_parameters.time_const); + _tecs.set_min_sink_rate(_parameters.min_sink_rate); + _tecs.set_max_sink_rate(_parameters.max_sink_rate); + _tecs.set_throttle_damp(_parameters.throttle_damp); + _tecs.set_integrator_gain(_parameters.integrator_gain); + _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); + _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); + _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); + _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_speed_weight(_parameters.speed_weight); + _tecs.set_pitch_damping(_parameters.pitch_damping); + _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); + _tecs.set_max_climb_rate(_parameters.max_climb_rate); + + /* sanity check parameters */ + if (_parameters.airspeed_max < _parameters.airspeed_min || + _parameters.airspeed_max < 5.0f || + _parameters.airspeed_min > 100.0f || + _parameters.airspeed_trim < _parameters.airspeed_min || + _parameters.airspeed_trim > _parameters.airspeed_max) { + warnx("error: airspeed parameters invalid"); + return 1; + } + + return OK; +} + +void +FixedwingPositionControl::vehicle_control_mode_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &vstatus_updated); + + if (vstatus_updated) { + + bool was_armed = _control_mode.flag_armed; + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + if (!was_armed && _control_mode.flag_armed) { + _launch_lat = _global_pos.lat / 1e7f; + _launch_lon = _global_pos.lon / 1e7f; + _launch_alt = _global_pos.alt; + _launch_valid = true; + } + } +} + +bool +FixedwingPositionControl::vehicle_airspeed_poll() +{ + /* check if there is an airspeed update or if it timed out */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + _airspeed_valid = true; + _airspeed_last_valid = hrt_absolute_time(); + return true; + + } else { + + /* no airspeed updates for one second */ + if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) { + _airspeed_valid = false; + } + } + + /* update TECS state */ + _tecs.enable_airspeed(_airspeed_valid); + + return false; +} + +void +FixedwingPositionControl::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + /* set rotation matrix */ + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _R_nb(i, j) = _att.R[i][j]; + } +} + +void +FixedwingPositionControl::vehicle_accel_poll() +{ + /* check if there is a new position */ + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + } +} + +void +FixedwingPositionControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool global_sp_updated; + orb_check(_global_set_triplet_sub, &global_sp_updated); + + if (global_sp_updated) { + orb_copy(ORB_ID(vehicle_global_position_set_triplet), _global_set_triplet_sub, &_global_triplet); + _setpoint_valid = true; + } +} + +void +FixedwingPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + l1_control::g_control->task_main(); +} + +float +FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) +{ + float airspeed; + + if (_airspeed_valid) { + airspeed = _airspeed.true_airspeed_m_s; + + } else { + airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f; + } + + /* cruise airspeed for all modes unless modified below */ + float target_airspeed = airspeed_demand; + + /* add minimum ground speed undershoot (only non-zero in presence of sufficient wind) */ + target_airspeed += _groundspeed_undershoot; + + if (0/* throttle nudging enabled */) { + //target_airspeed += nudge term. + } + + /* sanity check: limit to range */ + target_airspeed = math::constrain(target_airspeed, _parameters.airspeed_min, _parameters.airspeed_max); + + /* plain airspeed error */ + _airspeed_error = target_airspeed - airspeed; + + return target_airspeed; +} + +void +FixedwingPositionControl::calculate_gndspeed_undershoot() +{ + + if (_global_pos_valid) { + /* get ground speed vector */ + math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); + + /* rotate with current attitude */ + math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + yaw_vector.normalize(); + float ground_speed_body = yaw_vector * ground_speed_vector; + + /* + * Ground speed undershoot is the amount of ground velocity not reached + * by the plane. Consequently it is zero if airspeed is >= min ground speed + * and positive if airspeed < min ground speed. + * + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. + */ + _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + + } else { + _groundspeed_undershoot = 0; + } +} + +bool +FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, + const struct vehicle_global_position_set_triplet_s &global_triplet) +{ + bool setpoint = true; + + calculate_gndspeed_undershoot(); + + float eas2tas = 1.0f; // XXX calculate actual number based on current measurements + + // XXX re-visit + float baro_altitude = _global_pos.alt; + + /* filter speed and altitude for controller */ + math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + + _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); + float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + + /* no throttle limit as default */ + float throttle_max = 1.0f; + + /* AUTONOMOUS FLIGHT */ + + // XXX this should only execute if auto AND safety off (actuators active), + // else integrators should be constantly reset. + if (_control_mode.flag_control_position_enabled) { + + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + + /* execute navigation once we have a setpoint */ + if (_setpoint_valid) { + + /* current waypoint (the one currently heading for) */ + math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + + /* previous waypoint */ + math::Vector2f prev_wp; + + if (global_triplet.previous_valid) { + prev_wp.setX(global_triplet.previous.lat / 1e7f); + prev_wp.setY(global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid previous waypoint, go for the current wp. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(global_triplet.current.lat / 1e7f); + prev_wp.setY(global_triplet.current.lon / 1e7f); + + } + + // XXX add RTL switch + if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) { + + math::Vector2f rtl_pos(_launch_lat, _launch_lon); + + _l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _launch_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + // XXX handle case when having arrived at home (loiter) + + } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + global_triplet.current.loiter_direction, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { + + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -5.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ + // XXX this could make a great param + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + + if (altitude_error > -4.0f) { + + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, _parameters.throttle_max, throttle_land, + math::radians(-10.0f), math::radians(15.0f)); + + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + + /* limit roll motion to prevent wings from touching the ground first */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + } else { + + /* normal cruise speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + + } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { + + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + } + + // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), + // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), + // (double)next_wp.getX(), (double)next_wp.getY(), (global_triplet.previous_valid) ? "valid" : "invalid"); + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else if (_control_mode.flag_armed) { + + /* hold position, but only if armed, climb 20m in case this is engaged on ground level */ + + // XXX rework with smarter state machine + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt + 25.0f; + _loiter_hold = true; + } + + altitude_error = _loiter_hold_alt - _global_pos.alt; + + math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); + + /* loiter around current position */ + _l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius, + 1, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* climb with full throttle if the altitude error is bigger than 5 meters */ + bool climb_out = (altitude_error > 3); + + float min_pitch; + + if (climb_out) { + min_pitch = math::radians(20.0f); + + } else { + min_pitch = math::radians(_parameters.pitch_limit_min); + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _loiter_hold_alt, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + climb_out, min_pitch, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + + if (climb_out) { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + } + } + + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + + if (0/* switched from another mode to seatbelt */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* seatbelt on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + climb_out, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + setpoint = false; + } + + _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + + return setpoint; +} + +void +FixedwingPositionControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _global_set_triplet_sub = orb_subscribe(ORB_ID(vehicle_global_position_set_triplet)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_control_mode_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + /* abort on a nonzero return value from the parameter init */ + if (parameters_update()) { + /* parameter setup went wrong, abort */ + warnx("aborting startup due to errors."); + _task_should_exit = true; + } + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_control_mode_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + // XXX add timestamp check + _global_pos_valid = true; + + vehicle_attitude_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_airspeed_poll(); + // vehicle_baro_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* + * Attempt to control position, on success (= sensors present and not in manual mode), + * publish setpoint. + */ + if (control_position(current_position, ground_speed, _global_triplet)) { + _att_sp.timestamp = hrt_absolute_time(); + + /* lazily publish the setpoint only once available */ + if (_attitude_sp_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +FixedwingPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("fw_pos_control_l1", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4048, + (main_t)&FixedwingPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_pos_control_l1_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_pos_control_l1 {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (l1_control::g_control != nullptr) + errx(1, "already running"); + + l1_control::g_control = new FixedwingPositionControl; + + if (l1_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != l1_control::g_control->start()) { + delete l1_control::g_control; + l1_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (l1_control::g_control == nullptr) + errx(1, "not running"); + + delete l1_control::g_control; + l1_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (l1_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c new file mode 100644 index 000000000..3bb872405 --- /dev/null +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Controller parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); + + +PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); + + +PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); + + +PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); + + +PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); + +PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); + + +PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); + + +PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); + + +PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); + + +PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); + + +PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); + + +PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); + + +PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk new file mode 100644 index 000000000..b00b9aa5a --- /dev/null +++ b/src/modules/fw_pos_control_l1/module.mk @@ -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. +# +############################################################################ + +# +# Fixedwing L1 position control application +# + +MODULE_COMMAND = fw_pos_control_l1 + +SRCS = fw_pos_control_l1_main.cpp \ + fw_pos_control_l1_params.c diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 5eb7cba9b..7c10e297b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[]) case 'b': baudrate = strtoul(optarg, NULL, 10); - if (baudrate == 0) + if (baudrate < 9600 || baudrate > 921600) errx(1, "invalid baud rate '%s'", optarg); break; @@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); @@ -743,7 +743,7 @@ int mavlink_thread_main(int argc, char *argv[]) thread_running = false; - exit(0); + return 0; } static void @@ -767,7 +767,7 @@ int mavlink_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "mavlink already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink", @@ -776,15 +776,25 @@ int mavlink_main(int argc, char *argv[]) 2048, mavlink_thread_main, (const char **)argv); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "mavlink already stopped"); + thread_should_exit = true; while (thread_running) { usleep(200000); - printf("."); + warnx("."); } warnx("terminated."); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4674f7a24..c51a6de08 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; +struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; @@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1; static orb_advert_t pub_hil_mag = -1; static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; +static orb_advert_t pub_hil_battery = -1; + +/* packet counter */ +static int hil_counter = 0; +static int hil_frames = 0; +static uint64_t old_timestamp = 0; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; @@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg) vcmd.param5 = cmd_mavlink.param5; vcmd.param6 = cmd_mavlink.param6; vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; vcmd.target_system = cmd_mavlink.target_system; vcmd.target_component = cmd_mavlink.target_component; vcmd.source_system = msg->sysid; @@ -159,6 +167,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -207,7 +216,7 @@ handle_message(mavlink_message_t *msg) vcmd.param5 = 0; vcmd.param6 = 0; vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; vcmd.target_system = new_mode.target_system; vcmd.target_component = MAV_COMP_ID_ALL; vcmd.source_system = msg->sysid; @@ -360,11 +369,6 @@ handle_message(mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - /* packet counter */ - static uint16_t hil_counter = 0; - static uint16_t hil_frames = 0; - static uint64_t old_timestamp = 0; - /* sensors general */ hil_sensors.timestamp = hrt_absolute_time(); @@ -391,9 +395,9 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 /* adc */ - hil_sensors.adc_voltage_v[0] = 0; - hil_sensors.adc_voltage_v[1] = 0; - hil_sensors.adc_voltage_v[2] = 0; + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; /* magnetometer */ float mga2ga = 1.0e-3f; @@ -418,13 +422,13 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_counter = hil_counter; /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure; + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa hil_sensors.differential_pressure_counter = hil_counter; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - float ias = calc_indicated_airspeed(imu.diff_pressure); + float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); // XXX need to fix this float tas = ias; @@ -434,9 +438,11 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); /* individual sensor publications */ @@ -452,49 +458,72 @@ handle_message(mavlink_message_t *msg) if (pub_hil_gyro < 0) { pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); } struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); if (pub_hil_mag < 0) { pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); } struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); if (pub_hil_baro < 0) { pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); } @@ -502,17 +531,31 @@ handle_message(mavlink_message_t *msg) /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + // increment counters hil_counter++; hil_frames++; // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames / 10); old_timestamp = timestamp; hil_frames = 0; } @@ -537,9 +580,11 @@ handle_message(mavlink_message_t *msg) /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -552,6 +597,7 @@ handle_message(mavlink_message_t *msg) /* publish GPS measurement data */ if (pub_hil_gps > 0) { orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } @@ -570,10 +616,12 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + hil_global_pos.valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -586,6 +634,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } @@ -597,8 +646,8 @@ handle_message(mavlink_message_t *msg) /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); - + hil_attitude.R[i][j] = C_nb(i, j); + hil_attitude.R_valid = true; /* set quaternion */ @@ -620,25 +669,48 @@ handle_message(mavlink_message_t *msg) if (pub_hil_attitude > 0) { orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } + + /* fill in HIL battery status */ + hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + /* lazily publish the battery voltage */ + if (pub_hil_battery > 0) { + orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + + } else { + pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { @@ -705,17 +777,23 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -723,10 +801,10 @@ receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* Handle packet with waypoint component */ + /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - /* Handle packet with parameter component */ + /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } @@ -755,5 +833,7 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + + pthread_attr_destroy(&receiveloop_attr); return thread; } diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index be88b8794..e8d707948 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -167,9 +167,31 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->loiter_direction = (param3 >= 0) ? 1 : -1; sp->param1 = param1; - sp->param1 = param2; - sp->param1 = param3; - sp->param1 = param4; + sp->param2 = param2; + sp->param3 = param3; + sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 53d86ec00..f6860930c 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; @@ -99,6 +100,8 @@ struct listener { uintptr_t arg; }; +uint16_t cm_uint16_from_m_float(float m); + static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); static void l_vehicle_gps_position(const struct listener *l); @@ -120,6 +123,7 @@ static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -146,10 +150,24 @@ static const struct listener listeners[] = { {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + void l_sensor_combined(const struct listener *l) { @@ -235,8 +253,10 @@ l_vehicle_gps_position(const struct listener *l) /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = gps.cog_rad; + if (cog_deg > M_PI_F) cog_deg -= 2.0f * M_PI_F; + cog_deg *= M_RAD_TO_DEG_F; @@ -247,10 +267,10 @@ l_vehicle_gps_position(const struct listener *l) gps.lat, gps.lon, gps.alt, - gps.eph_m * 1e2f, // from m to cm - gps.epv_m * 1e2f, // from m to cm + cm_uint16_from_m_float(gps.eph_m), + cm_uint16_from_m_float(gps.epv_m), gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from rad to deg * 100 + cog_deg * 1e2f, // from deg to deg * 100 gps.satellites_visible); /* update SAT info every 10 seconds */ @@ -674,11 +694,24 @@ l_airspeed(const struct listener *l) mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); + +} + static void * uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, @@ -820,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); @@ -829,5 +867,7 @@ uorb_receive_start(void) pthread_t thread; pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + + pthread_attr_destroy(&uorb_attr); return thread; } diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e2e630046..91773843b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -65,7 +65,9 @@ #include <uORB/topics/telemetry_status.h> #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> +#include <uORB/topics/battery_status.h> #include <drivers/drv_rc_input.h> +#include <uORB/topics/navigation_capabilities.h> struct mavlink_subscriptions { int sensor_sub; @@ -91,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -101,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 16a7c2d35..7e4a2688f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -246,55 +246,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm->size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm->size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - /* * Calculate distance in global frame. * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - static uint16_t counter; if (seq < wpm->size) { mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); @@ -315,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float float dxy = radius_earth * c; float dz = alt - wp->z; + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -337,17 +296,27 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); - return sqrt(dx * dx + dy * dy + dz * dz); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + + return sqrtf(dx * dx + dy * dy + dz * dz); } else { return -1.0f; } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { + /* nothing to check here, return */ + return; + } + if (wpm->current_active_wp_id < wpm->size) { float orbit; @@ -366,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit) { + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; } + // check if required yaw reached float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); @@ -417,17 +397,70 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } if (time_elapsed) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + if (cur_wp->autocontinue) { + cur_wp->current = 0; + float navigation_lat = -1.0f; + float navigation_lon = -1.0f; + float navigation_alt = -1.0f; + int navigation_frame = -1; + + /* initialize to current position in case we don't find a suitable navigation waypoint */ + if (global_pos->valid) { + navigation_lat = global_pos->lat/1e7; + navigation_lon = global_pos->lon/1e7; + navigation_alt = global_pos->alt; + navigation_frame = MAV_FRAME_GLOBAL; + } else if (local_pos->xy_valid && local_pos->z_valid) { + navigation_lat = local_pos->x; + navigation_lon = local_pos->y; + navigation_alt = local_pos->z; + navigation_frame = MAV_FRAME_LOCAL_NED; + } + + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated restart the waypoint list from the beginning - */ - wpm->current_active_wp_id = 0; + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ + if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { + + /* this is a navigation waypoint */ + navigation_frame = cur_wp->frame; + navigation_lat = cur_wp->x; + navigation_lon = cur_wp->y; + navigation_alt = cur_wp->z; + } + + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } + + /* we risk an endless loop for missions without navigation waypoints, abort. */ + break; } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) @@ -459,7 +492,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -482,12 +515,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - // Do NOT continously send the current WP, since we're not loosing messages - // if (now - wpm->timestamp_last_send_setpoint > wpm->delay_setpoint && wpm->current_active_wp_id < wpm->size) { - // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); - // } - - check_waypoints_reached(now, global_position, local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -498,115 +526,6 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { -// case MAVLINK_MSG_ID_ATTITUDE: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) -// { -// mavlink_attitude_t att; -// mavlink_msg_attitude_decode(msg, &att); -// float yaw_tolerance = wpm->accept_range_yaw; -// //compare current yaw -// if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*FM_PI) -// { -// if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) -// wpm->yaw_reached = true; -// } -// else if(att.yaw - yaw_tolerance < 0.0f) -// { -// float lowerBound = 360.0f + att.yaw - yaw_tolerance; -// if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) -// wpm->yaw_reached = true; -// } -// else -// { -// float upperBound = att.yaw + yaw_tolerance - 2.f*FM_PI; -// if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) -// wpm->yaw_reached = true; -// } -// } -// } -// break; -// } -// -// case MAVLINK_MSG_ID_LOCAL_POSITION_NED: -// { -// if(msg->sysid == mavlink_system.sysid && wpm->current_active_wp_id < wpm->size) -// { -// mavlink_mission_item_t *wp = &(wpm->waypoints[wpm->current_active_wp_id]); -// -// if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) -// { -// mavlink_local_position_ned_t pos; -// mavlink_msg_local_position_ned_decode(msg, &pos); -// //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); -// -// wpm->pos_reached = false; -// -// // compare current position (given in message) with current waypoint -// float orbit = wp->param1; -// -// float dist; -//// if (wp->param2 == 0) -//// { -//// // FIXME segment distance -//// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -//// } -//// else -//// { -// dist = mavlink_wpm_distance_to_point(wpm->current_active_wp_id, pos.x, pos.y, pos.z); -//// } -// -// if (dist >= 0.f && dist <= orbit && wpm->yaw_reached) -// { -// wpm->pos_reached = true; -// } -// } -// } -// break; -// } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm->size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; @@ -617,26 +536,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#endif wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -664,13 +573,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm->current_active_wp_id); -#endif wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); @@ -678,33 +582,16 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi wpm->timestamp_firstinside_orbit = 0; } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - -#endif } } else { -#ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - -#endif } break; @@ -1151,5 +1038,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd30..d7d6b31dc 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include <stdbool.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/navigation_capabilities.h> // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index e71344982..0edb53a3e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf if (strcmp(uart_name, "/dev/ttyACM0") != OK) { /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); close(uart); return -1; } @@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n" + " mavlink_onboard stop\n" + " mavlink_onboard status\n"); exit(1); } @@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink_onboard", @@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[]) while (thread_running) { usleep(200000); } - warnx("terminated."); + warnx("terminated"); exit(0); } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0236e6126..4658bcc1d 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); + warnx("terminating..."); fflush(stdout); usleep(50000); @@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* create command */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } - do { - nread = read(uart_fd, &ch, 1); + /* non-blocking read. read may return negative values */ + nread = read(uart_fd, buf, sizeof(buf)); - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); } - } while (nread > 0); + } } } @@ -319,8 +333,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 04582f2a4..60a211817 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -89,18 +89,18 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_status_s status; @@ -108,29 +108,16 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* - * Do not rate-limit the loop to prevent aliasing - * if rate-limiting would be desired later, the line below would - * enable it. - * - * rate-limit the attitude subscription to 200Hz to pace our loop - * orb_set_interval(att_sub, 5); - */ - struct pollfd fds[2] = { - { .fd = att_sub, .events = POLLIN }, - { .fd = param_sub, .events = POLLIN } - }; + /* subscribe */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { @@ -146,233 +133,252 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - /* welcome user */ warnx("starting"); /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 2, 500); + int ret = poll(fds, 1, 500); if (ret < 0) { /* poll error, count it in perf */ perf_count(mc_err_perf); - } else if (ret == 0) { - /* no return value, ignore */ - } else { + } else if (ret > 0) { + /* only run controller if attitude changed */ + perf_begin(mc_loop_perf); - /* only update parameters if they changed */ - if (fds[1].revents & POLLIN) { - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), param_sub, &update); + /* attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + + bool updated; + + /* parameters */ + orb_check(parameter_update_sub, &updated); + if (updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ } - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { + /* control mode */ + orb_check(vehicle_control_mode_sub, &updated); - perf_begin(mc_loop_perf); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); + } - /* get a local copy of system state */ - bool updated; - orb_check(control_mode_sub, &updated); + /* manual control setpoint */ + orb_check(manual_control_setpoint_sub, &updated); - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + } - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of attitude setpoint */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - /* get a local copy of rates setpoint */ - orb_check(setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); - } + /* attitude setpoint */ + orb_check(vehicle_attitude_setpoint_sub, &updated); - /* get a local copy of status */ - orb_check(status_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + } - if (updated) { - orb_copy(ORB_ID(vehicle_status), status_sub, &status); - } + /* offboard control setpoint */ + orb_check(offboard_control_setpoint_sub, &updated); - /* get a local copy of the current sensor values */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); + } - /* set flag to safe value */ - control_yaw_position = true; + /* vehicle status */ + orb_check(vehicle_status_sub, &updated); - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } + if (updated) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + } - /* define which input is the dominating control input */ - if (control_mode.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + /* sensors */ + orb_check(sensor_combined_sub, &updated); - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + } - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; + /* set flag to safe value */ + control_yaw_position = true; - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } + /* define which input is the dominating control input */ + if (control_mode.flag_control_offboard_enabled) { + /* offboard inputs */ + if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { + rates_sp.roll = offboard_sp.p1; + rates_sp.pitch = offboard_sp.p2; + rates_sp.yaw = offboard_sp.p3; + rates_sp.thrust = offboard_sp.p4; + rates_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } else { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } + } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { + att_sp.roll_body = offboard_sp.p1; + att_sp.pitch_body = offboard_sp.p2; + att_sp.yaw_body = offboard_sp.p3; + att_sp.thrust = offboard_sp.p4; + att_sp.timestamp = hrt_absolute_time(); + /* publish the result to the vehicle actuators */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + } else if (control_mode.flag_control_manual_enabled) { + /* manual input */ + if (control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; + + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = true; } - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } + } else { + /* only move yaw setpoint if manual input is != 0 */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { + /* control yaw rate */ + control_yaw_position = false; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; + } else { + control_yaw_position = true; } + } - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } + } - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { - if (!control_mode.flag_control_auto_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } + /* manual rate inputs (ACRO), from RC control or joystick */ + if (control_mode.flag_control_rates_enabled) { + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } - /* reset yaw setpoint after non-manual control */ + /* reset yaw setpoint after ACRO */ reset_yaw_sp = true; } - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } } - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } + + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(rates_sp_sub, &rates_sp_updated); + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); + } - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); - } + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + /* run rates controller if needed */ + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_updated = false; + orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - actuators.control[3] = 0.0f; + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); } - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* apply controller */ + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; + } - perf_end(mc_loop_perf); - } /* end of poll call for attitude updates */ - } /* end of poll return value check */ + /* fill in manual control values */ + actuators.control[4] = manual.flaps; + actuators.control[5] = manual.aux1; + actuators.control[6] = manual.aux2; + actuators.control[7] = manual.aux3; + + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + + perf_end(mc_loop_perf); + } } warnx("stopping, disarming motors"); @@ -383,10 +389,9 @@ mc_thread_main(int argc, char *argv[]) orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(att_sub); - close(control_mode_sub); - close(manual_sub); + close(vehicle_attitude_sub); + close(vehicle_control_mode_sub); + close(manual_control_setpoint_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index c78232f11..8245aa560 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -62,15 +62,15 @@ #include <systemlib/param/param.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); //PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 0a336be47..adb63186c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -59,14 +59,14 @@ #include <systemlib/err.h> #include <drivers/drv_hrt.h> -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); //PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.0f); /* 0.15 F405 Flamewheel */ -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); //PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 336523072..36dd370fb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -193,7 +193,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&global_pos_sp, 0, sizeof(local_pos_sp)); + memset(&global_pos_sp, 0, sizeof(global_pos_sp)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); @@ -212,17 +212,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool global_pos_sp_reproject = false; + bool reset_mission_sp = false; bool global_pos_sp_valid = false; - bool local_pos_sp_valid = false; - bool reset_sp_z = true; - bool reset_sp_xy = true; + bool reset_man_sp_z = true; + bool reset_man_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; - bool reset_integral = true; - bool reset_auto_pos = true; + bool reset_auto_sp_xy = true; + bool reset_auto_sp_z = true; + bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; @@ -270,11 +270,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* use integral_limit_out = tilt_max / 2 */ float i_limit; - if (params.xy_vel_i == 0.0f) { + if (params.xy_vel_i > 0.0f) { i_limit = params.tilt_max / params.xy_vel_i / 2.0f; } else { - i_limit = 1.0f; // not used really + i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); @@ -297,7 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); global_pos_sp_valid = true; - global_pos_sp_reproject = true; + reset_mission_sp = true; } hrt_abstime t = hrt_absolute_time(); @@ -312,8 +312,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_auto_sp_z = true; + reset_auto_sp_xy = true; + reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -348,8 +351,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } @@ -371,8 +374,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); @@ -405,41 +408,45 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; - /* local position setpoint is valid and can be used for loiter after position controlled mode */ - local_pos_sp_valid = control_mode.flag_control_position_enabled; - - reset_auto_pos = true; + /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ + reset_auto_sp_xy = !control_mode.flag_control_position_enabled; + reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; + reset_takeoff_sp = true; /* force reprojection of global setpoint after manual mode */ - global_pos_sp_reproject = true; + reset_mission_sp = true; } else if (control_mode.flag_control_auto_enabled) { /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_auto_pos) { + if (reset_takeoff_sp) { + reset_takeoff_sp = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; - reset_auto_pos = false; mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } + reset_auto_sp_xy = false; + reset_auto_sp_z = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; + reset_mission_sp = true; local_ref_timestamp = local_pos.ref_timestamp; double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; @@ -447,9 +454,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } - if (global_pos_sp_reproject) { + if (reset_mission_sp) { + reset_mission_sp = false; /* update global setpoint projection */ - global_pos_sp_reproject = false; if (global_pos_sp_valid) { /* global position setpoint valid, use it */ @@ -464,40 +471,50 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } - - local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { - if (!local_pos_sp_valid) { + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; /* local position setpoint is invalid, * use current position as setpoint for loiter */ local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; + } + + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + local_pos_sp.z = local_pos.z; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + reset_takeoff_sp = true; } if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - global_pos_sp_reproject = true; + reset_mission_sp = true; } + /* copy yaw setpoint to vehicle_local_position_setpoint topic */ + local_pos_sp.yaw = att_sp.yaw_body; + /* reset setpoints after AUTO mode */ - reset_sp_xy = true; - reset_sp_z = true; + reset_man_sp_xy = true; + reset_man_sp_z = true; } else { - /* no control, loiter or stay on ground */ + /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ @@ -508,27 +525,30 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } - reset_sp_z = true; + reset_man_sp_z = true; } else { /* in air: hold altitude */ - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } + + reset_auto_sp_z = false; } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } + + reset_auto_sp_xy = false; } } @@ -540,7 +560,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { - reset_sp_z = true; + reset_man_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -558,7 +578,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } else { - reset_sp_xy = true; + reset_man_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -640,12 +660,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; - global_pos_sp_reproject = true; - reset_auto_pos = true; + reset_mission_sp = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index bf9578ea3..b7041e4d5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk new file mode 100644 index 000000000..0404b06c7 --- /dev/null +++ b/src/modules/navigator/module.mk @@ -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. +# +############################################################################ + +# +# Main Navigation Controller +# + +MODULE_COMMAND = navigator + +SRCS = navigator_main.cpp \ + navigator_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp new file mode 100644 index 000000000..f6c44444a --- /dev/null +++ b/src/modules/navigator/navigator_main.cpp @@ -0,0 +1,604 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 navigator_main.c + * Implementation of the main navigation state machine. + * + * Handles missions, geo fencing and failsafe navigation behavior. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_set_triplet.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/mission.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +/** + * navigator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int navigator_main(int argc, char *argv[]); + +class Navigator +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the sensors task. + */ + ~Navigator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _att_sub; /**< vehicle attitude subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + + orb_advert_t _triplet_pub; /**< position setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_global_position_set_triplet_s _global_triplet; /**< triplet of global setpoints */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + struct { + float throttle_cruise; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t throttle_cruise; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for position updates. + */ + void vehicle_attitude_poll(); + + /** + * Check for set triplet updates. + */ + void mission_poll(); + + /** + * Control throttle. + */ + float control_throttle(float energy_error); + + /** + * Control pitch. + */ + float control_pitch(float altitude_error); + + void calculate_airspeed_errors(); + void calculate_gndspeed_undershoot(); + void calculate_altitude_error(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace navigator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Navigator *g_navigator; +} + +Navigator::Navigator() : + + _task_should_exit(false), + _navigator_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _att_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _triplet_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), +/* states */ + _mission_items_maxcount(20), + _mission_valid(false), + _loiter_hold(false) +{ + _mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount); + if (!_mission_items) { + _mission_items_maxcount = 0; + warnx("no free RAM to allocate mission, rejecting any waypoints"); + } + + _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + + /* fetch initial parameter values */ + parameters_update(); +} + +Navigator::~Navigator() +{ + if (_navigator_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_navigator_task); + break; + } + } while (_navigator_task != -1); + } + + navigator::g_navigator = nullptr; +} + +int +Navigator::parameters_update() +{ + + //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + return OK; +} + +void +Navigator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +Navigator::vehicle_attitude_poll() +{ + /* check if there is a new position */ + bool att_updated; + orb_check(_att_sub, &att_updated); + + if (att_updated) { + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + } +} + +void +Navigator::mission_poll() +{ + /* check if there is a new setpoint */ + bool mission_updated; + orb_check(_mission_sub, &mission_updated); + + if (mission_updated) { + + struct mission_s mission; + orb_copy(ORB_ID(mission), _mission_sub, &mission); + + // XXX this is not optimal yet, but a first prototype / + // test implementation + + if (mission.count <= _mission_items_maxcount) { + /* + * Perform an atomic copy & state update + */ + irqstate_t flags = irqsave(); + + memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s)); + _mission_valid = true; + + irqrestore(flags); + } else { + warnx("mission larger than storage space"); + } + } +} + +void +Navigator::task_main_trampoline(int argc, char *argv[]) +{ + navigator::g_navigator->task_main(); +} + +void +Navigator::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _mission_sub = orb_subscribe(ORB_ID(mission)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + /* rate limit position updates to 50 Hz */ + orb_set_interval(_global_pos_sub, 20); + + parameters_update(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _global_pos_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if position changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); + + vehicle_attitude_poll(); + + mission_poll(); + + math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + // Current waypoint + math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + // Global position + math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + + /* AUTONOMOUS FLIGHT */ + + if (1 /* autonomous flight */) { + + /* execute navigation once we have a setpoint */ + if (_mission_valid) { + + // Next waypoint + math::Vector2f prev_wp; + + if (_global_triplet.previous_valid) { + prev_wp.setX(_global_triplet.previous.lat / 1e7f); + prev_wp.setY(_global_triplet.previous.lon / 1e7f); + + } else { + /* + * No valid next waypoint, go for heading hold. + * This is automatically handled by the L1 library. + */ + prev_wp.setX(_global_triplet.current.lat / 1e7f); + prev_wp.setY(_global_triplet.current.lon / 1e7f); + + } + + + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + // XXX to be put in its own class + + if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { + /* waypoint is a plain navigation waypoint */ + + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + + /* waypoint is a loiter waypoint */ + + } + + // XXX at this point we always want no loiter hold if a + // mission is active + _loiter_hold = false; + + } else { + + if (!_loiter_hold) { + _loiter_hold_lat = _global_pos.lat / 1e7f; + _loiter_hold_lon = _global_pos.lon / 1e7f; + _loiter_hold_alt = _global_pos.alt; + _loiter_hold = true; + } + + //_parameters.loiter_hold_radius + } + + } else if (0/* seatbelt mode enabled */) { + + /** SEATBELT FLIGHT **/ + continue; + + } else { + + /** MANUAL FLIGHT **/ + + /* no flight mode applies, do not publish an attitude setpoint */ + continue; + } + + /******** MAIN NAVIGATION STATE MACHINE ********/ + + if (_global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + // XXX define launch position and return + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_LAND) { + // XXX flared descent on final approach + + } else if (_global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + + /* apply minimum pitch if altitude has not yet been reached */ + if (_global_pos.alt < _global_triplet.current.altitude) { + _att_sp.pitch_body = math::max(_att_sp.pitch_body, _global_triplet.current.param1); + } + } + + /* lazily publish the setpoint only once available */ + if (_triplet_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position_set_triplet), _triplet_pub, &_global_triplet); + + } else { + /* advertise and publish */ + _triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &_global_triplet); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _navigator_task = -1; + _exit(0); +} + +int +Navigator::start() +{ + ASSERT(_navigator_task == -1); + + /* start the task */ + _navigator_task = task_spawn_cmd("navigator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Navigator::task_main_trampoline, + nullptr); + + if (_navigator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int navigator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: navigator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (navigator::g_navigator != nullptr) + errx(1, "already running"); + + navigator::g_navigator = new Navigator; + + if (navigator::g_navigator == nullptr) + errx(1, "alloc failed"); + + if (OK != navigator::g_navigator->start()) { + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (navigator::g_navigator == nullptr) + errx(1, "not running"); + + delete navigator::g_navigator; + navigator::g_navigator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (navigator::g_navigator) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c new file mode 100644 index 000000000..06df9a452 --- /dev/null +++ b/src/modules/navigator/navigator_params.c @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 navigator_params.c + * + * Parameters defined by the navigator task. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Navigator parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); + diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 932f61088..10007bf96 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.global_z = true; + local_pos.z_global = true; } } } @@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.timestamp = t; local_pos.xy_valid = can_estimate_xy && gps_valid; local_pos.v_xy_valid = can_estimate_xy; - local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.global_xy; + global_pos.valid = local_pos.xy_global; - if (local_pos.global_xy) { + if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = (int32_t)(est_lat * 1e7); @@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.relative_alt = -local_pos.z; } - if (local_pos.global_z) { + if (local_pos.z_global) { global_pos.alt = local_pos.ref_alt - local_pos.z; } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 796c6cd9f..5c621cfb2 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -108,9 +108,11 @@ controls_tick() { perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, PX4IO_CONTROL_CHANNELS /* XXX this should be INPUT channels, once untangled */); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + r_raw_rc_count = 8; + } perf_end(c_gather_sbus); /* @@ -124,8 +126,6 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. @@ -286,7 +286,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH)) override = true; if (override) { diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 206e27db5..fd3b72015 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,28 +243,35 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; if (dsm_fd < 0) return; -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - // XXX implement - #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 -#else switch (cmd) { case dsm_bind_power_down: /*power down DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif break; case dsm_bind_power_up: /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif dsm_guess_format(true); break; @@ -278,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, false); up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); } break; @@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (dsm_channel_shift == 11) + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ *num_values |= 0x8000; + } /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. @@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Upon receiving a full dsm frame we attempt to decode it. * * @param[out] values pointer to per channel array of decoded values - * @param[out] num_values pointer to number of raw channel values returned + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @return true=decoded raw channel values updated, false=no update */ bool diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 10aeb5c9f..79b6546b3 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -149,12 +149,6 @@ interface_init(void) #endif } -void -interface_tick() -{ -} - - /* reset the I2C bus used to recover from lockups diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0edd91b24..05897b4ce 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -47,6 +47,7 @@ #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <systemlib/mixer/mixer.h> extern "C" { @@ -59,12 +60,6 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 -/* - * Time that the ESCs need to initialize - */ - #define ESC_INIT_TIME_US 1000000 - #define ESC_RAMP_TIME_US 2000000 - /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -76,15 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static uint64_t esc_init_time; - -enum esc_state_e { - ESC_OFF, - ESC_INIT, - ESC_RAMP, - ESC_ON -}; -static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -166,6 +152,30 @@ mixer_tick(void) } /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + * XXX correct behaviour for failsafe may require an additional case + * here. + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { @@ -184,107 +194,15 @@ mixer_tick(void) float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; - uint16_t ramp_promille; - - /* update esc init state, but only if we are truely armed and not just PWM enabled */ - if (mixer_servos_armed && should_arm) { - - switch (esc_state) { - - /* after arming, some ESCs need an initalization period, count the time from here */ - case ESC_OFF: - esc_init_time = hrt_absolute_time(); - esc_state = ESC_INIT; - break; - - /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ - case ESC_INIT: - if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { - esc_state = ESC_RAMP; - } - break; - - /* then ramp until the min speed is reached */ - case ESC_RAMP: - if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { - esc_state = ESC_ON; - } - break; - - case ESC_ON: - default: - - break; - } - } else { - esc_state = ESC_OFF; - } - - /* do the calculations during the ramp for all at once */ - if(esc_state == ESC_RAMP) { - ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; - } - - /* mix */ mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); - /* scale to PWM and update the servo outputs as required */ - for (unsigned i = 0; i < mixed; i++) { - - /* save actuator values for FMU readback */ - r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - - switch (esc_state) { - case ESC_INIT: - r_page_servos[i] = (outputs[i] * 600 + 1500); - break; - - case ESC_RAMP: - r_page_servos[i] = (outputs[i] - * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 - + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); - break; - - case ESC_ON: - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); - break; + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); - case ESC_OFF: - default: - break; - } - } for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -298,7 +216,6 @@ mixer_tick(void) mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> PWM disabled"); - } if (mixer_servos_armed && should_arm) { @@ -307,9 +224,9 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); } else if (mixer_servos_armed && should_always_enable_pwm) { - /* set the idle servo outputs. */ + /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) - up_pwm_servo_set(i, r_page_servo_idle[i]); + up_pwm_servo_set(i, r_page_servo_disarmed[i]); } } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 59f470a94..01869569f 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -14,6 +14,7 @@ SRCS = adc.c \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ + ../systemlib/pwm_limit/pwm_limit.c ifeq ($(BOARD),px4io-v1) SRCS += i2c.c diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f5fa0ba75..5e5396782 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -165,11 +165,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#endif #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ #define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ @@ -218,8 +220,8 @@ enum { /* DSM bind states */ /* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ -/* PWM idle values that are active, even when SAFETY_SAFE */ -#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM disarmed values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index e70b3fe88..ff9eecd74 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -50,6 +50,7 @@ #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> +#include <systemlib/pwm_limit/pwm_limit.h> #include <stm32_uart.h> @@ -64,6 +65,8 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; +pwm_limit_t pwm_limit; + /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -159,9 +162,6 @@ user_start(int argc, char *argv[]) /* start the FMU interface */ interface_init(); - /* add a performance counter for the interface */ - perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); - /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -174,6 +174,9 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + /* initialize PWM limit lib */ + pwm_limit_init(&pwm_limit); + #if 0 /* not enough memory, lock down */ if (minfo.mxordblk < 500) { @@ -203,11 +206,6 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); - /* kick the interface */ - perf_begin(interface_perf); - interface_tick(); - perf_end(interface_perf); - /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -218,6 +216,7 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); +#if 0 /* check for debug activity */ show_debug_messages(); @@ -234,6 +233,7 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } +#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea67043e..4fea0288c 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -46,12 +46,14 @@ #include "protocol.h" +#include <systemlib/pwm_limit/pwm_limit.h> + /* * Constants and limits. */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 +#define PX4IO_INPUT_CHANNELS 8 // XXX this should be 18 channels /* * Debug logging @@ -80,7 +82,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ -extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ +extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ /* * Register aliases. @@ -100,7 +102,9 @@ extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) @@ -121,6 +125,11 @@ struct sys_state_s { extern struct sys_state_s system_state; /* + * PWM limit structure + */ +extern pwm_limit_t pwm_limit; + +/* * GPIO handling. */ #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) @@ -200,7 +209,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c5..40597adf1 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -145,7 +145,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, #else @@ -197,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /** * PAGE 106 @@ -205,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; * minimum PWM values when armed * */ -uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN }; /** * PAGE 107 @@ -213,15 +215,15 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90 * maximum PWM values when armed * */ -uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX }; /** * PAGE 108 * - * idle PWM values for difficult ESCs + * disarmed PWM values for difficult ESCs * */ -uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; +uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) @@ -274,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - /* XXX range-check value? */ - r_page_servo_failsafe[offset] = *values; + if (*values == 0) { + /* ignore 0 */ + } else if (*values < PWM_MIN) { + r_page_servo_failsafe[offset] = PWM_MIN; + } else if (*values > PWM_MAX) { + r_page_servo_failsafe[offset] = PWM_MAX; + } else { + r_page_servo_failsafe[offset] = *values; + } /* flag the failsafe values as custom */ r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; @@ -291,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_min[offset] = 900; - - else if (*values > 1200) - r_page_servo_control_min[offset] = 1200; - else if (*values < 900) - r_page_servo_control_min[offset] = 900; - else + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_HIGHEST_MIN) { + r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_MIN) { + r_page_servo_control_min[offset] = PWM_MIN; + } else { r_page_servo_control_min[offset] = *values; + } offset++; num_values--; @@ -313,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - if (*values == 0) - /* set to default */ - r_page_servo_control_max[offset] = 2100; - - else if (*values > 2100) - r_page_servo_control_max[offset] = 2100; - else if (*values < 1800) - r_page_servo_control_max[offset] = 1800; - else + if (*values == 0) { + /* ignore 0 */ + } else if (*values > PWM_MAX) { + r_page_servo_control_max[offset] = PWM_MAX; + } else if (*values < PWM_LOWEST_MAX) { + r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; + } offset++; num_values--; @@ -330,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - case PX4IO_PAGE_IDLE_PWM: - - /* copy channel data */ - while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { - - if (*values == 0) - /* set to default */ - r_page_servo_idle[offset] = 0; - - else if (*values < 900) - r_page_servo_idle[offset] = 900; - else if (*values > 2100) - r_page_servo_idle[offset] = 2100; - else - r_page_servo_idle[offset] = *values; + case PX4IO_PAGE_DISARMED_PWM: + { + /* flag for all outputs */ + bool all_disarmed_off = true; + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) { + /* 0 means disabling always PWM */ + r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_MIN) { + r_page_servo_disarmed[offset] = PWM_MIN; + all_disarmed_off = false; + } else if (*values > PWM_MAX) { + r_page_servo_disarmed[offset] = PWM_MAX; + all_disarmed_off = false; + } else { + r_page_servo_disarmed[offset] = *values; + all_disarmed_off = false; + } - /* flag the failsafe values as custom */ - r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + offset++; + num_values--; + values++; + } - offset++; - num_values--; - values++; + if (all_disarmed_off) { + /* disable PWM output if disarmed */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { + /* enable PWM output always */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + } } break; @@ -462,22 +481,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; -#ifdef POWER_RELAY1 POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); -#endif -#ifdef POWER_RELAY2 POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); -#endif -#ifdef POWER_ACC1 POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); -#endif -#ifdef POWER_ACC2 POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); -#endif break; +#endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; @@ -678,27 +691,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * XXX pending measurements - * - * slope = xxx - * intercept = xxx - * - * Intercept corrected for best results @ 5.0V. - */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; } } #endif - /* XXX PX4IO_P_STATUS_VRSSI */ /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); @@ -773,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; - case PX4IO_PAGE_IDLE_PWM: - SELECT_PAGE(r_page_servo_idle); + case PX4IO_PAGE_DISARMED_PWM: + SELECT_PAGE(r_page_servo_disarmed); break; default: diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 073ddeaba..c523df6ca 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -66,7 +66,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_channels); int sbus_init(const char *device) @@ -97,7 +97,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values) +sbus_input(uint16_t *values, uint16_t *num_values, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -154,7 +154,7 @@ sbus_input(uint16_t *values, uint16_t *num_values) * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values); + return sbus_decode(now, values, num_values, max_channels); } /* @@ -194,7 +194,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { @@ -214,8 +214,8 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) /* we have received something we think is a frame */ last_frame_time = frame_time; - unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? - SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; + unsigned chancount = (max_values > SBUS_INPUT_CHANNELS) ? + SBUS_INPUT_CHANNELS : max_values; /* use the decoder matrix to extract channel data */ for (unsigned channel = 0; channel < chancount; channel++) { diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index 94d7407df..e9adc8cd6 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -74,9 +74,6 @@ static DMA_HANDLE rx_dma; static int serial_interrupt(int irq, void *context); static void dma_reset(void); -/* if we spend this many ticks idle, reset the DMA */ -static unsigned idle_ticks; - static struct IOPacket dma_packet; /* serial register accessors */ @@ -150,16 +147,6 @@ interface_init(void) debug("serial init"); } -void -interface_tick() -{ - /* XXX look for stuck/damaged DMA and reset? */ - if (idle_ticks++ > 100) { - dma_reset(); - idle_ticks = 0; - } -} - static void rx_handle_packet(void) { @@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) /* disable UART DMA */ rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); - /* reset the idle counter */ - idle_ticks = 0; - /* handle the received packet */ rx_handle_packet(); @@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context) /* it was too short - possibly truncated */ perf_count(pc_badidle); + dma_reset(); return 0; } @@ -343,7 +328,8 @@ dma_reset(void) sizeof(dma_packet), DMA_CCR_MINC | DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS); + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIVERYHI); /* start receive DMA ready for the next packet */ stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e83fb7dd3..f94875d5b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,7 @@ #include <systemlib/err.h> #include <unistd.h> #include <drivers/drv_hrt.h> +#include <math.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -84,12 +85,14 @@ #include <uORB/topics/esc_status.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <mavlink/mavlink_log.h> #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" +#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -180,8 +183,17 @@ static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -static void write_formats(int fd); +static int write_formats(int fd); +/** + * Write version message to log file. + */ +static int write_version(int fd); + +/** + * Write parameters to log file. + */ +static int write_parameters(int fd); static bool file_exist(const char *filename); @@ -237,11 +249,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -354,10 +366,13 @@ static void *logwriter_thread(void *arg) struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; - int log_file = open_logfile(); + int log_fd = open_logfile(); - /* write log messages formats */ - write_formats(log_file); + /* write log messages formats, version and parameters */ + log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; @@ -396,7 +411,7 @@ static void *logwriter_thread(void *arg) n = available; } - n = write(log_file, read_ptr, n); + n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; @@ -411,21 +426,23 @@ static void *logwriter_thread(void *arg) } else { n = 0; + /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { break; } + should_wait = true; } if (++poll_count == 10) { - fsync(log_file); + fsync(log_fd); poll_count = 0; } } - fsync(log_file); - close(log_file); + fsync(log_fd); + close(log_fd); return OK; } @@ -479,34 +496,92 @@ void sdlog2_stop_log() /* wait for write thread to return */ int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { warnx("error joining logwriter thread: %i", ret); } + logwriter_pthread = 0; sdlog2_status(); } - -void write_formats(int fd) +int write_formats(int fd) { /* construct message format packet */ struct { LOG_PACKET_HEADER; struct log_format_s body; - } log_format_packet = { + } log_msg_format = { LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG), }; - /* fill message format packet for each format and write to log */ - int i; + int written = 0; - for (i = 0; i < log_formats_num; i++) { - log_format_packet.body = log_formats[i]; - log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); + /* fill message format packet for each format and write it */ + for (int i = 0; i < log_formats_num; i++) { + log_msg_format.body = log_formats[i]; + written += write(fd, &log_msg_format, sizeof(log_msg_format)); } - fsync(fd); + return written; +} + +int write_version(int fd) +{ + /* construct version message */ + struct { + LOG_PACKET_HEADER; + struct log_VER_s body; + } log_msg_VER = { + LOG_PACKET_HEADER_INIT(LOG_VER_MSG), + }; + + /* fill version message and write it */ + strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); + return write(fd, &log_msg_VER, sizeof(log_msg_VER)); +} + +int write_parameters(int fd) +{ + /* construct parameter message */ + struct { + LOG_PACKET_HEADER; + struct log_PARM_s body; + } log_msg_PARM = { + LOG_PACKET_HEADER_INIT(LOG_PARM_MSG), + }; + + int written = 0; + param_t params_cnt = param_count(); + + for (param_t param = 0; param < params_cnt; param++) { + /* fill parameter message and write it */ + strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name)); + float value = NAN; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: { + int32_t i; + param_get(param, &i); + value = i; // cast integer to float + break; + } + + case PARAM_TYPE_FLOAT: + param_get(param, &value); + break; + + default: + break; + } + + log_msg_PARM.body.value = value; + written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM)); + } + + return written; } int sdlog2_thread_main(int argc, char *argv[]) @@ -583,6 +658,16 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } + const char *converter_in = "/etc/logging/conv.zip"; + char *converter_out = malloc(120); + sprintf(converter_out, "%s/conv.zip", folder_path); + + if (file_copy(converter_in, converter_out)) { + errx(1, "unable to copy conversion scripts, exiting."); + } + + free(converter_out); + /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -594,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + memset(&buf_status, 0, sizeof(buf_status)); /* warning! using union here to save memory, elements should be used separately! */ @@ -619,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; + memset(&buf, 0, sizeof(buf)); struct { @@ -789,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); fds[fdsc_count].fd = subs.airspeed_sub; fds[fdsc_count].events = POLLIN; - fdsc_count++; + fdsc_count++; /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); @@ -877,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[]) continue; } - ifds = 1; // Begin from fds[1] again + ifds = 1; // begin from fds[1] again pthread_mutex_lock(&logbuffer_mutex); @@ -1046,6 +1133,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } @@ -1133,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESCs --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - for (uint8_t i=0; i<buf.esc.esc_count; i++) - { + + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { log_msg.msg_type = LOG_ESC_MSG; log_msg.body.log_ESC.counter = buf.esc.counter; log_msg.body.log_ESC.esc_count = buf.esc.esc_count; @@ -1248,7 +1338,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return ret; + return OK; } void handle_command(struct vehicle_command_s *cmd) @@ -1282,6 +1372,7 @@ void handle_status(struct vehicle_status_s *status) { // TODO use flag from actuator_armed here? bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { flag_system_armed = armed; diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index 5c175ef7e..dc5e6c8bd 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -85,10 +85,10 @@ struct log_format_s { #define LOG_FORMAT(_name, _format, _labels) { \ .type = LOG_##_name##_MSG, \ - .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ - .name = #_name, \ - .format = _format, \ - .labels = _labels \ + .length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ } #define LOG_FORMAT_MSG 0x80 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 4eeb65a87..90093a407 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -48,12 +48,6 @@ /* define message formats */ #pragma pack(push, 1) -/* --- TIME - TIME STAMP --- */ -#define LOG_TIME_MSG 1 -struct log_TIME_s { - uint64_t t; -}; - /* --- ATT - ATTITUDE --- */ #define LOG_ATT_MSG 2 struct log_ATT_s { @@ -109,6 +103,9 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; + uint8_t xy_flags; + uint8_t z_flags; + uint8_t landed; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -250,17 +247,36 @@ struct log_GVSP_s { float vz; }; +/* --- TIME - TIME STAMP --- */ +#define LOG_TIME_MSG 129 +struct log_TIME_s { + uint64_t t; +}; + +/* --- VER - VERSION --- */ +#define LOG_VER_MSG 130 +struct log_VER_s { + char arch[16]; + char fw_git[64]; +}; + +/* --- PARM - PARAMETER --- */ +#define LOG_PARM_MSG 131 +struct log_PARM_s { + char name[16]; + float value; +}; + #pragma pack(pop) /* construct list of all message formats */ - static const struct log_format_s log_formats[] = { - LOG_FORMAT(TIME, "Q", "StartTime"), + /* business-level messages, ID < 0x80 */ LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), @@ -274,6 +290,11 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + /* system-level messages, ID >= 0x80 */ + // FMT: don't write format of format message, it's useless + LOG_FORMAT(TIME, "Q", "StartTime"), + LOG_FORMAT(VER, "NZ", "Arch,FwGit"), + LOG_FORMAT(PARM, "Nf", "Name,Value"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h new file mode 100644 index 000000000..c6a9ba638 --- /dev/null +++ b/src/modules/sdlog2/sdlog2_version.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * 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 sdlog2_version.h + * + * Tools for system version detection. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef SDLOG2_VERSION_H_ +#define SDLOG2_VERSION_H_ + +/* + GIT_VERSION is defined at build time via a Makefile call to the + git command line. + */ +#define FREEZE_STR(s) #s +#define STRINGIFY(s) FREEZE_STR(s) +#define FW_GIT STRINGIFY(GIT_VERSION) + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#define HW_ARCH "PX4FMU_V1" +#endif + +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#define HW_ARCH "PX4FMU_V2" +#endif + +#endif /* SDLOG2_VERSION_H_ */ diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445..96a443c6e 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 992abf2cc..587b81588 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,8 +44,34 @@ #include <systemlib/param/param.h> +/** + * Gyro X offset FIXME + * + * This is an X-axis offset for the gyro. + * Adjust it according to the calibration data. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); + +/** + * Gyro Y offset FIXME with dot. + * + * @min -10.0 + * @max 10.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); + +/** + * Gyro Z offset FIXME + * + * @min -5.0 + * @max 5.0 + * @group Gyro Config + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); @@ -164,9 +190,10 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); - +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ -PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ +#endif +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); @@ -174,7 +201,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); @@ -191,8 +219,9 @@ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ -PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e98c4d548..da0c11372 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -67,14 +67,15 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <conversion/rotation.h> -#include <systemlib/ppm_decode.h> #include <systemlib/airspeed.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/battery_status.h> @@ -105,22 +106,22 @@ * IN13 - aux1 * IN14 - aux2 * IN15 - pressure sensor - * + * * IO: * IN4 - servo supply rail * IN5 - analog RSSI */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - #define ADC_BATTERY_VOLTAGE_CHANNEL 10 - #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - #define ADC_BATTERY_VOLTAGE_CHANNEL 2 - #define ADC_BATTERY_CURRENT_CHANNEL 3 - #define ADC_5V_RAIL_SENSE 4 - #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_5V_RAIL_SENSE 4 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif #define BAT_VOL_INITIAL 0.f @@ -134,82 +135,9 @@ */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define PPM_INPUT_TIMEOUT_INTERVAL 50000 /**< 50 ms timeout / 20 Hz */ - #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** - * 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 } -}; - -/** * Sensor app start / stop handling function * * @ingroup apps @@ -239,12 +167,12 @@ public: private: static const unsigned _rc_max_chan_count = RC_CHANNELS_MAX; /**< maximum number of r/c channels we handle */ - hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */ + hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */ /** - * Gather and publish PPM input data. + * Gather and publish RC input data. */ - void ppm_poll(); + void rc_poll(); /* XXX should not be here - should be own driver */ int _fd_adc; /**< ADC driver handle */ @@ -269,6 +197,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -390,11 +319,6 @@ private: int parameters_update(); /** - * Get the rotation matrices - */ - void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); - - /** * Do accel-related initialisation. */ void accel_init(); @@ -501,7 +425,7 @@ Sensors *g_sensors = nullptr; } Sensors::Sensors() : - _ppm_last_valid(0), + _rc_last_valid(0), _fd_adc(-1), _last_adc(0), @@ -524,6 +448,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -532,8 +457,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3,3), - _external_mag_rotation(3,3), + _board_rotation(3, 3), + _external_mag_rotation(3, 3), _mag_is_external(false) { @@ -660,9 +585,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -674,19 +599,19 @@ Sensors::parameters_update() tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; - + /* handle blowup in the scaling factor calculation */ if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f) ) { + (tmpRevFactor > 0.2f)) { warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; + + } else { + _parameters.scaling_factor[i] = tmpScaleFactor; } - else { - _parameters.scaling_factor[i] = tmpScaleFactor; - } } /* handle wrong values */ @@ -809,24 +734,6 @@ Sensors::parameters_update() } void -Sensors::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); -} - -void Sensors::accel_init() { int fd; @@ -841,7 +748,7 @@ Sensors::accel_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the accel internal sampling rate up to at leat 1000Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 1000); @@ -849,7 +756,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -857,10 +764,10 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else - #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#else +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 - #endif +#endif warnx("using system accel"); close(fd); @@ -882,7 +789,7 @@ Sensors::gyro_init() // XXX do the check more elegantly - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) @@ -892,7 +799,7 @@ Sensors::gyro_init() if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) ioctl(fd, SENSORIOCSPOLLRATE, 800); - #else +#else /* set the gyro internal sampling rate up to at least 760Hz */ ioctl(fd, GYROIOCSSAMPLERATE, 760); @@ -900,7 +807,7 @@ Sensors::gyro_init() /* set the driver to poll at 760Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 760); - #endif +#endif warnx("using system gyro"); close(fd); @@ -924,23 +831,28 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { /* set the pollrate accordingly */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ if (ret == OK) { /* set the driver to poll also at the slower rate */ ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { errx(1, "FATAL: mag sampling rate could not be set"); } } - + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) @@ -993,7 +905,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); raw.accelerometer_m_s2[1] = vect(1); @@ -1019,7 +931,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); raw.gyro_rad_s[1] = vect(1); @@ -1047,9 +959,9 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; if (_mag_is_external) - vect = _external_mag_rotation*vect; + vect = _external_mag_rotation * vect; else - vect = _board_rotation*vect; + vect = _board_rotation * vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1094,8 +1006,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar*1e2f, - raw.baro_pres_mbar*1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1224,6 +1136,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { @@ -1233,12 +1148,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { - raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); } /* look for specific channels and process the raw voltage to measurement data */ @@ -1266,12 +1181,12 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else { _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); } - } + } } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) + float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on @@ -1303,22 +1218,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } void -Sensors::ppm_poll() +Sensors::rc_poll() { + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - struct pollfd fds[1]; - fds[0].fd = _rc_sub; - fds[0].events = POLLIN; - /* check non-blocking for new data */ - int poll_ret = poll(fds, 1, 0); - - if (poll_ret > 0) { + if (rc_updated) { + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); struct manual_control_setpoint_s manual_control; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1349,7 +1261,7 @@ Sensors::ppm_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _ppm_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1359,6 +1271,7 @@ Sensors::ppm_poll() */ if (rc_input.values[i] < _parameters.min[i]) rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) rc_input.values[i] = _parameters.max[i]; @@ -1485,6 +1398,16 @@ Sensors::ppm_poll() manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); } + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + /* check if ready for publishing */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1501,6 +1424,14 @@ Sensors::ppm_poll() } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } + + /* check if ready for publishing */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } } @@ -1516,8 +1447,7 @@ Sensors::task_main() { /* inform about start */ - printf("[sensors] Initializing..\n"); - fflush(stdout); + warnx("Initializing.."); /* start individual sensors */ accel_init(); @@ -1620,7 +1550,7 @@ Sensors::task_main() orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); /* Look for new r/c input data */ - ppm_poll(); + rc_poll(); perf_end(_loop_perf); } @@ -1638,11 +1568,11 @@ Sensors::start() /* start the task */ _sensors_task = task_spawn_cmd("sensors_task", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2048, - (main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Sensors::task_main_trampoline, + nullptr); if (_sensors_task < 0) { warn("task start failed"); diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp index df0dfe838..cce46bf5f 100644 --- a/src/modules/systemlib/mixer/mixer.cpp +++ b/src/modules/systemlib/mixer/mixer.cpp @@ -50,6 +50,8 @@ #include <stdio.h> #include <math.h> #include <unistd.h> +#include <ctype.h> +#include <systemlib/err.h> #include "mixer.h" @@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler) return 0; } +const char * +Mixer::findtag(const char *buf, unsigned &buflen, char tag) +{ + while (buflen >= 2) { + if ((buf[0] == tag) && (buf[1] == ':')) + return buf; + buf++; + buflen--; + } + return nullptr; +} + +const char * +Mixer::skipline(const char *buf, unsigned &buflen) +{ + const char *p; + + /* if we can find a CR or NL in the buffer, skip up to it */ + if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) { + /* skip up to it AND one beyond - could be on the NUL symbol now */ + buflen -= (p - buf) + 1; + return p + 1; + } + + return nullptr; +} + /****************************************************************************/ NullMixer::NullMixer() : @@ -142,6 +171,22 @@ NullMixer * NullMixer::from_text(const char *buf, unsigned &buflen) { NullMixer *nm = nullptr; + const char *end = buf + buflen; + + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + return nm; + } + + } if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) { nm = new NullMixer; diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index bbfa130a9..723bf9f3b 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -128,7 +128,9 @@ #ifndef _SYSTEMLIB_MIXER_MIXER_H #define _SYSTEMLIB_MIXER_MIXER_H value +#include <nuttx/config.h> #include "drivers/drv_mixer.h" +#include "mixer_load.h" /** * Abstract class defining a mixer mixing zero or more inputs to @@ -210,6 +212,24 @@ protected: */ static int scale_check(struct mixer_scaler_s &scaler); + /** + * Find a tag + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @param tag character to search for. + */ + static const char * findtag(const char *buf, unsigned &buflen, char tag); + + /** + * Skip a line + * + * @param buf The buffer to operate on. + * @param buflen length of the buffer. + * @return 0 / OK if a line could be skipped, 1 else + */ + static const char * skipline(const char *buf, unsigned &buflen); + private: }; @@ -239,6 +259,11 @@ public: void reset(); /** + * Count the mixers in the group. + */ + unsigned count(); + + /** * Adds mixers to the group based on a text description in a buffer. * * Mixer definitions begin with a single capital letter and a colon. diff --git a/src/modules/systemlib/mixer/mixer_group.cpp b/src/modules/systemlib/mixer/mixer_group.cpp index 1dbc512cd..3ed99fba0 100644 --- a/src/modules/systemlib/mixer/mixer_group.cpp +++ b/src/modules/systemlib/mixer/mixer_group.cpp @@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space) return index; } +unsigned +MixerGroup::count() +{ + Mixer *mixer = _first; + unsigned index = 0; + + while ((mixer != nullptr)) { + mixer = mixer->_next; + index++; + } + + return index; +} + void MixerGroup::groups_required(uint32_t &groups) { @@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen) /* only adjust buflen if parsing was successful */ buflen = resid; + debug("SUCCESS - buflen: %d", buflen); } else { /* diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c new file mode 100644 index 000000000..a55ddf8a3 --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 mixer_load.c + * + * Programmable multi-channel mixer library. + */ + +#include <nuttx/config.h> +#include <string.h> +#include <stdio.h> +#include <ctype.h> + +#include "mixer_load.h" + +int load_mixer_file(const char *fname, char *buf, unsigned maxlen) +{ + FILE *fp; + char line[120]; + + /* open the mixer definition file */ + fp = fopen(fname, "r"); + if (fp == NULL) { + return 1; + } + + /* read valid lines from the file into a buffer */ + buf[0] = '\0'; + for (;;) { + + /* get a line, bail on error/EOF */ + line[0] = '\0'; + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* if the line doesn't look like a mixer definition line, skip it */ + if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) + continue; + + /* compact whitespace in the buffer */ + char *t, *f; + for (f = line; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } + + /* if the line is too long to fit in the buffer, bail */ + if ((strlen(line) + strlen(buf) + 1) >= maxlen) { + return 1; + } + + /* add the line to the buffer */ + strcat(buf, line); + } + + return 0; +} + diff --git a/src/modules/systemlib/mixer/mixer_load.h b/src/modules/systemlib/mixer/mixer_load.h new file mode 100644 index 000000000..4b7091d5b --- /dev/null +++ b/src/modules/systemlib/mixer/mixer_load.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 mixer_load.h + * + */ + + +#ifndef _SYSTEMLIB_MIXER_LOAD_H +#define _SYSTEMLIB_MIXER_LOAD_H value + +#include <nuttx/config.h> + +__BEGIN_DECLS + +__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 8ded0b05c..b89f341b6 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = { { 1.000000, 0.000000, -1.00 }, { -1.000000, 0.000000, -1.00 }, }; -const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], &_config_quad_v[0], @@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME &_config_octa_x[0], &_config_octa_plus[0], }; -const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = { +const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ 4, /* quad_plus */ 4, /* quad_v */ @@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; + const char *end = buf + buflen; + + /* enforce that the mixer ends with space or a new line */ + for (int i = buflen - 1; i >= 0; i--) { + if (buf[i] == '\0') + continue; + + /* require a space or newline at the end of the buffer, fail on printable chars */ + if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { + /* found a line ending or space, so no split symbols / numbers. good. */ + break; + } else { + debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]); + return nullptr; + } + + } if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) { debug("multirotor parse failed on '%s'", buf); @@ -188,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } if (used > (int)buflen) { - debug("multirotor spec used %d of %u", used, buflen); + debug("OVERFLOW: multirotor spec used %d of %u", used, buflen); + return nullptr; + } + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); return nullptr; } - buflen -= used; + debug("remaining in buf: %d, first char: %c", buflen, buf[0]); if (!strcmp(geomname, "4+")) { geometry = MultirotorMixer::QUAD_PLUS; diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp index 07dc5f37f..c3985b5de 100644 --- a/src/modules/systemlib/mixer/mixer_simple.cpp +++ b/src/modules/systemlib/mixer/mixer_simple.cpp @@ -71,44 +71,30 @@ SimpleMixer::~SimpleMixer() free(_info); } -static const char * -findtag(const char *buf, unsigned &buflen, char tag) -{ - while (buflen >= 2) { - if ((buf[0] == tag) && (buf[1] == ':')) - return buf; - buf++; - buflen--; - } - return nullptr; -} - -static void -skipline(const char *buf, unsigned &buflen) -{ - const char *p; - - /* if we can find a CR or NL in the buffer, skip up to it */ - if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) - buflen -= (p - buf); -} - int SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler) { int ret; int s[5]; + int n = -1; buf = findtag(buf, buflen, 'O'); - if ((buf == nullptr) || (buflen < 12)) + if ((buf == nullptr) || (buflen < 12)) { + debug("output parser failed finding tag, ret: '%s'", buf); return -1; + } - if ((ret = sscanf(buf, "O: %d %d %d %d %d", - &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) { - debug("scaler parse failed on '%s' (got %d)", buf, ret); + if ((ret = sscanf(buf, "O: %d %d %d %d %d %n", + &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) { + debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n); + return -1; + } + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); return -1; } - skipline(buf, buflen); scaler.negative_scale = s[0] / 10000.0f; scaler.positive_scale = s[1] / 10000.0f; @@ -126,15 +112,22 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale int s[5]; buf = findtag(buf, buflen, 'S'); - if ((buf == nullptr) || (buflen < 16)) + if ((buf == nullptr) || (buflen < 16)) { + debug("control parser failed finding tag, ret: '%s'", buf); return -1; + } if (sscanf(buf, "S: %u %u %d %d %d %d %d", &u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) { debug("control parse failed on '%s'", buf); return -1; } - skipline(buf, buflen); + + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + return -1; + } control_group = u[0]; control_index = u[1]; @@ -162,7 +155,11 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c goto out; } - buflen -= used; + buf = skipline(buf, buflen); + if (buf == nullptr) { + debug("no line ending, line is incomplete"); + goto out; + } mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs)); @@ -173,22 +170,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c mixinfo->control_count = inputs; - if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) + if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) { + debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf); goto out; + } for (unsigned i = 0; i < inputs; i++) { if (parse_control_scaler(end - buflen, buflen, mixinfo->controls[i].scaler, mixinfo->controls[i].control_group, - mixinfo->controls[i].control_index)) + mixinfo->controls[i].control_index)) { + debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf); goto out; + } + } sm = new SimpleMixer(control_cb, cb_handle, mixinfo); if (sm != nullptr) { mixinfo = nullptr; - debug("loaded mixer with %d inputs", inputs); + debug("loaded mixer with %d input(s)", inputs); } else { debug("could not allocate memory for mixer"); diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk index 4d45e1c50..fc7485e20 100644 --- a/src/modules/systemlib/mixer/module.mk +++ b/src/modules/systemlib/mixer/module.mk @@ -39,4 +39,5 @@ LIBNAME = mixerlib SRCS = mixer.cpp \ mixer_group.cpp \ mixer_multirotor.cpp \ - mixer_simple.cpp + mixer_simple.cpp \ + mixer_load.c diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index c69de52b7..398657dd7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include <unistd.h> #include <systemlib/err.h> #include <errno.h> +#include <semaphore.h> #include <sys/stat.h> @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_sem = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ @@ -505,34 +508,63 @@ param_get_default_file(void) int param_save_default(void) { - int result; - - /* delete the file in case it exists */ - struct stat buffer; - if (stat(param_get_default_file(), &buffer) == 0) { - result = unlink(param_get_default_file()); - if (result != OK) - warnx("unlinking file %s failed.", param_get_default_file()); + int res; + int fd; + + const char *filename = param_get_default_file(); + const char *filename_tmp = malloc(strlen(filename) + 5); + sprintf(filename_tmp, "%s.tmp", filename); + + /* delete temp file if exist */ + res = unlink(filename_tmp); + + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete temp file: %s", filename_tmp); + + if (res == OK) { + /* write parameters to temp file */ + fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("failed to open temp file: %s", filename_tmp); + res = ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) + warnx("failed to write parameters to file: %s", filename_tmp); + } + + close(fd); } - /* create the file */ - int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + if (res == OK) { + /* delete parameters file */ + res = unlink(filename); - if (fd < 0) { - warn("opening '%s' for writing failed", param_get_default_file()); - return fd; + if (res != OK && errno == ENOENT) + res = OK; + + if (res != OK) + warn("failed to delete parameters file: %s", filename); } - result = param_export(fd, false); - close(fd); + if (res == OK) { + /* rename temp file to parameters */ + res = rename(filename_tmp, filename); - if (result != 0) { - warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); - return result; + if (res != OK) + warn("failed to rename %s to %s", filename_tmp, filename); } - return 0; + free(filename_tmp); + + return res; } /** diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287..bf84b7945 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a1..e1e3cbe95 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 739503ed4..77c952f52 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0.0f; + } - } else { - if (!isfinite(i)) { - i = 0.0f; + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c new file mode 100644 index 000000000..cac3dc82a --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> + * + * 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 pwm_limit.c + * + * Lib to limit PWM output + * + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include "pwm_limit.h" +#include <math.h> +#include <stdbool.h> +#include <drivers/drv_hrt.h> + +void pwm_limit_init(pwm_limit_t *limit) +{ + limit->state = LIMIT_STATE_OFF; + limit->time_armed = 0; + return; +} + +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +{ + /* first evaluate state changes */ + switch (limit->state) { + case LIMIT_STATE_OFF: + if (armed) + limit->state = LIMIT_STATE_RAMP; + limit->time_armed = hrt_absolute_time(); + break; + case LIMIT_STATE_INIT: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) + limit->state = LIMIT_STATE_RAMP; + break; + case LIMIT_STATE_RAMP: + if (!armed) + limit->state = LIMIT_STATE_OFF; + else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) + limit->state = LIMIT_STATE_ON; + break; + case LIMIT_STATE_ON: + if (!armed) + limit->state = LIMIT_STATE_OFF; + break; + default: + break; + } + + unsigned progress; + uint16_t temp_pwm; + + /* then set effective_pwm based on state */ + switch (limit->state) { + case LIMIT_STATE_OFF: + case LIMIT_STATE_INIT: + for (unsigned i=0; i<num_channels; i++) { + effective_pwm[i] = disarmed_pwm[i]; + output[i] = 0.0f; + } + break; + case LIMIT_STATE_RAMP: + + progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; + for (unsigned i=0; i<num_channels; i++) { + + uint16_t ramp_min_pwm; + + /* if a disarmed pwm value was set, blend between disarmed and min */ + if (disarmed_pwm[i] > 0) { + + /* safeguard against overflows */ + uint16_t disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) + disarmed = min_pwm[i]; + + uint16_t disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + output[i] = (float)progress/10000.0f * output[i]; + } + break; + case LIMIT_STATE_ON: + for (unsigned i=0; i<num_channels; i++) { + effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + /* effective_output stays the same */ + } + break; + default: + break; + } + return; +} diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h new file mode 100644 index 000000000..9974770be --- /dev/null +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> + * + * 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 pwm_limit.h + * + * Lib to limit PWM output + * + * @author Julian Oes <joes@student.ethz.ch> + */ + +#ifndef PWM_LIMIT_H_ +#define PWM_LIMIT_H_ + +#include <stdint.h> +#include <stdbool.h> + +/* + * time for the ESCs to initialize + * (this is not actually needed if PWM is sent right after boot) + */ +#define INIT_TIME_US 500000 +/* + * time to slowly ramp up the ESCs + */ +#define RAMP_TIME_US 2500000 + +typedef struct { + enum { + LIMIT_STATE_OFF = 0, + LIMIT_STATE_INIT, + LIMIT_STATE_RAMP, + LIMIT_STATE_ON + } state; + uint64_t time_armed; +} pwm_limit_t; + +__BEGIN_DECLS + +__EXPORT void pwm_limit_init(pwm_limit_t *limit); + +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); + +__END_DECLS + +#endif /* PWM_LIMIT_H_ */ diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 60d6473b8..b4350cc24 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,14 +47,12 @@ #include <mavlink/mavlink_log.h> #include <uORB/topics/rc_channels.h> -int rc_calibration_check(void) { +int rc_calibration_check(int mavlink_fd) { char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, _parameter_handles_rev, _parameter_handles_dz; - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - float param_min, param_max, param_trim, param_rev, param_dz; /* first check channel mappings */ diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e2238d151..e70b83cce 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(void); +__EXPORT int rc_calibration_check(int mavlink_fd); __END_DECLS diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1eb63a799..3514dca24 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s); #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 30895ca83..446140423 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -60,7 +60,7 @@ struct actuator_outputs_s { uint64_t timestamp; /**< output timestamp in us since system boot */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ - int noutputs; /**< valid outputs */ + unsigned noutputs; /**< valid outputs */ }; /** diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764..e4d2c92ce 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h new file mode 100644 index 000000000..55668790b --- /dev/null +++ b/src/modules/uORB/topics/servorail_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 + * 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 servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 686fd765c..1a245132a 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 31ff014de..e6e4743c6 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -86,7 +86,8 @@ enum VEHICLE_CMD VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_ENUM_END=401, /* | */ + VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END=501, /* | */ }; /** @@ -119,7 +120,7 @@ struct vehicle_command_s float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ - uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ + enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ uint8_t target_component; /**< Component which should execute the command, 0 for all components */ uint8_t source_system; /**< System sending the command */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d..38fb74c9b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,6 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 0fc0ed5c9..143734e37 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,17 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + bool valid; /**< true if position satisfies validity criteria of estimator */ int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d..a56434d3b 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 0a7fb4e9d..1639a08c2 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -56,9 +56,9 @@ struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ + int32_t lat; /**< Latitude in 1E-7 degrees */ + int32_t lon; /**< Longitude in 1E-7 degrees */ + int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 31a0e632b..427153782 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -70,8 +70,8 @@ struct vehicle_local_position_s /* Heading */ float yaw; /* Reference position in GPS / WGS84 frame */ - bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 188fa4e37..1ea02d81e 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +46,7 @@ #include <stdbool.h> #include <unistd.h> #include <fcntl.h> +#include <poll.h> #include <sys/mount.h> #include <sys/ioctl.h> #include <sys/stat.h> @@ -63,41 +65,46 @@ static void usage(const char *reason); __EXPORT int esc_calib_main(int argc, char *argv[]); -#define MAX_CHANNELS 8 +#define MAX_CHANNELS 14 static void usage(const char *reason) { if (reason != NULL) warnx("%s", reason); - errx(1, - "usage:\n" - "esc_calib [-d <device>] <channels>\n" - " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " <channels> Provide channels (e.g.: 1 2 3 4)\n" - ); + + errx(1, + "usage:\n" + "esc_calib [-d <device>] <channels>\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <channels> Provide channels (e.g.: 1 2 3 4)\n" + ); } int esc_calib_main(int argc, char *argv[]) { - const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *dev = PWM_OUTPUT_DEVICE_PATH; char *ep; bool channels_selected[MAX_CHANNELS] = {false}; int ch; int ret; char c; + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + if (argc < 2) usage(NULL); - while ((ch = getopt(argc, argv, "d:")) != EOF) { + while ((ch = getopt(argc - 1, argv, "d:")) != EOF) { switch (ch) { - + case 'd': dev = optarg; - argc--; + argc -= 2; break; default: @@ -105,7 +112,7 @@ esc_calib_main(int argc, char *argv[]) } } - if(argc < 1) { + if (argc < 2) { usage("no channels provided"); } @@ -116,135 +123,150 @@ esc_calib_main(int argc, char *argv[]) if (*ep == '\0') { if (channel_number > MAX_CHANNELS || channel_number <= 0) { err(1, "invalid channel number: %d", channel_number); + } else { - channels_selected[channel_number-1] = true; + channels_selected[channel_number - 1] = true; } } } - /* Wait for confirmation */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); - - warnx("ATTENTION, please remove or fix props before starting calibration!\n" - "\n" - "Also press the arming switch first for safety off\n" - "\n" - "Do you really want to start calibration: y or n?\n"); + printf("\nATTENTION, please remove or fix propellers before starting calibration!\n" + "\n" + "Make sure\n" + "\t - that the ESCs are not powered\n" + "\t - that safety is off (two short blinks)\n" + "\t - that the controllers are stopped\n" + "\n" + "Do you want to start calibration now: y or n?\n"); /* wait for user input */ while (1) { - - if (read(console, &c, 1) == 1) { + + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 'y' || c == 'Y') { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); - close(console); + printf("ESC calibration exited\n"); exit(0); + } else if (c == 'n' || c == 'N') { - warnx("ESC calibration aborted"); - close(console); + printf("ESC calibration aborted\n"); exit(0); + } else { - warnx("Unknown input, ESC calibration aborted"); - close(console); + printf("Unknown input, ESC calibration aborted\n"); exit(0); - } + } } + /* rate limit to ~ 20 Hz */ usleep(50000); } /* open for ioctl only */ int fd = open(dev, 0); + if (fd < 0) err(1, "can't open %s", dev); - // XXX arming not necessaire at the moment - // /* Then arm */ - // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - // if (ret != OK) - // err(1, "PWM_SERVO_SET_ARM_OK"); + /* get max PWM value setting */ + uint16_t pwm_max = 0; + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max); - // ret = ioctl(fd, PWM_SERVO_ARM, 0); - // if (ret != OK) - // err(1, "PWM_SERVO_ARM"); + if (ret != OK) + err(1, "PWM_SERVO_GET_MAX_PWM"); + /* get disarmed PWM value setting */ + uint16_t pwm_disarmed = 0; + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed); - + if (ret != OK) + err(1, "PWM_SERVO_GET_DISARMED_PWM"); - /* Wait for user confirmation */ - warnx("Set high PWM\n" - "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); + /* wait for user confirmation */ + printf("\nHigh PWM set: %d\n" + "\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" + "\n", pwm_max); + fflush(stdout); while (1) { + /* set max PWM */ + for (unsigned i = 0; i < MAX_CHANNELS; i++) { + if (channels_selected[i]) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max); - /* First set high PWM */ - for (unsigned i = 0; i<MAX_CHANNELS; i++) { - if(channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), 2100); if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { warnx("ESC calibration exited"); - close(console); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - /* we don't need any more user input */ - - - warnx("Set low PWM, hit ENTER when finished"); + printf("Low PWM set: %d\n" + "\n" + "Hit ENTER when finished\n" + "\n", pwm_disarmed); while (1) { - /* Then set low PWM */ - for (unsigned i = 0; i<MAX_CHANNELS; i++) { - if(channels_selected[i]) { - ret = ioctl(fd, PWM_SERVO_SET(i), 900); + /* set disarmed PWM */ + for (unsigned i = 0; i < MAX_CHANNELS; i++) { + if (channels_selected[i]) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed); + if (ret != OK) err(1, "PWM_SERVO_SET(%d)", i); } } - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + if (c == 13) { - + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { - warnx("ESC calibration exited"); - close(console); + printf("ESC calibration exited\n"); exit(0); } } + /* rate limit to ~ 20 Hz */ usleep(50000); } - - warnx("ESC calibration finished"); - close(console); - - // XXX disarming not necessaire at the moment - /* Now disarm again */ - // ret = ioctl(fd, PWM_SERVO_DISARM, 0); - - + printf("ESC calibration finished\n"); exit(0); -}
\ No newline at end of file +} diff --git a/src/systemcmds/mixer/mixer.c b/src/systemcmds/mixer/mixer.cpp index e642ed067..6da39d371 100644 --- a/src/systemcmds/mixer/mixer.c +++ b/src/systemcmds/mixer/mixer.cpp @@ -47,10 +47,15 @@ #include <nuttx/compiler.h> #include <systemlib/err.h> -#include <drivers/drv_mixer.h> +#include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> -__EXPORT int mixer_main(int argc, char *argv[]); +/** + * Mixer utility for loading mixer files to devices + * + * @ingroup apps + */ +extern "C" __EXPORT int mixer_main(int argc, char *argv[]); static void usage(const char *reason) noreturn_function; static void load(const char *devname, const char *fname) noreturn_function; @@ -87,8 +92,6 @@ static void load(const char *devname, const char *fname) { int dev; - FILE *fp; - char line[120]; char buf[2048]; /* open the device */ @@ -99,49 +102,7 @@ load(const char *devname, const char *fname) if (ioctl(dev, MIXERIOCRESET, 0)) err(1, "can't reset mixers on %s", devname); - /* open the mixer definition file */ - fp = fopen(fname, "r"); - if (fp == NULL) - err(1, "can't open %s", fname); - - /* read valid lines from the file into a buffer */ - buf[0] = '\0'; - for (;;) { - - /* get a line, bail on error/EOF */ - line[0] = '\0'; - if (fgets(line, sizeof(line), fp) == NULL) - break; - - /* if the line doesn't look like a mixer definition line, skip it */ - if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) - continue; - - /* compact whitespace in the buffer */ - char *t, *f; - for (f = buf; *f != '\0'; f++) { - /* scan for space characters */ - if (*f == ' ') { - /* look for additional spaces */ - t = f + 1; - while (*t == ' ') - t++; - if (*t == '\0') { - /* strip trailing whitespace */ - *f = '\0'; - } else if (t > (f + 1)) { - memmove(f + 1, t, strlen(t) + 1); - } - } - } - - /* if the line is too long to fit in the buffer, bail */ - if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) - break; - - /* add the line to the buffer */ - strcat(buf, line); - } + load_mixer_file(fname, &buf[0], sizeof(buf)); /* XXX pass the buffer to the device */ int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf); diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index d5a3f9f77..cdbff75f0 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -36,7 +36,7 @@ # MODULE_COMMAND = mixer -SRCS = mixer.c +SRCS = mixer.cpp MODULE_STACKSIZE = 4096 diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index f5bd01ce8..1c58a2db6 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - bool rc_ok = (OK == rc_calibration_check()); + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); /* warn */ if (!rc_ok) @@ -163,7 +163,7 @@ system_eval: warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); fflush(stderr); - int buzzer = open("/dev/tone_alarm", O_WRONLY); + int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { @@ -227,4 +227,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -}
\ No newline at end of file +} diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c42a36c7f..09a934400 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -45,6 +45,7 @@ #include <stdbool.h> #include <unistd.h> #include <fcntl.h> +#include <poll.h> #include <sys/mount.h> #include <sys/ioctl.h> #include <sys/stat.h> @@ -71,16 +72,36 @@ usage(const char *reason) warnx("%s", reason); errx(1, "usage:\n" - "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n" - " -v Print information about the PWM device\n" - " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" - " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" - " <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n" - " arm | disarm Arm or disarm the ouptut\n" - " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n" - " <chanmask> Directly supply alt rate channel mask (debug use only)\n" + "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + " arm Arm output\n" + " disarm Disarm output\n" + "\n" + " rate ... Configure PWM rates\n" + " [-g <channel group>] Channel group that should update at the alternate rate\n" + " [-m <chanmask> ] Directly supply channel mask\n" + " [-a] Configure all outputs\n" + " -r <alt_rate> PWM rate (50 to 400 Hz)\n" + "\n" + " failsafe ... Configure failsafe PWM values\n" + " disarmed ... Configure disarmed PWM values\n" + " min ... Configure minimum PWM values\n" + " max ... Configure maximum PWM values\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Configure all outputs\n" + " -p <pwm value> PWM value\n" + "\n" + " test ... Directly set PWM values\n" + " [-c <channels>] Supply channels (e.g. 1234)\n" + " [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n" + " [-a] Configure all outputs\n" + " -p <pwm value> PWM value\n" + "\n" + " info Print information about the PWM device\n" + "\n" + " -v Print verbose information\n" + " -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -92,199 +113,406 @@ pwm_main(int argc, char *argv[]) unsigned alt_rate = 0; uint32_t alt_channel_groups = 0; bool alt_channels_set = false; - bool print_info = false; + bool print_verbose = false; int ch; int ret; char *ep; + uint32_t set_mask = 0; unsigned group; - int32_t set_mask = -1; + unsigned long channels; + unsigned single_ch = 0; + unsigned pwm_value = 0; - if (argc < 2) + if (argc < 1) usage(NULL); - while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) { + while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { switch (ch) { + + case 'd': + if (NULL == strstr(optarg, "/dev/")) { + warnx("device %s not valid", optarg); + usage(NULL); + } + dev = optarg; + break; + + case 'v': + print_verbose = true; + break; + case 'c': + /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + channels = strtoul(optarg, &ep, 0); + + while ((single_ch = channels % 10)) { + + set_mask |= 1<<(single_ch-1); + channels /= 10; + } + break; + + case 'g': group = strtoul(optarg, &ep, 0); if ((*ep != '\0') || (group >= 32)) usage("bad channel_group value"); alt_channel_groups |= (1 << group); alt_channels_set = true; + warnx("alt channels set, group: %d", group); break; - case 'd': - dev = optarg; + case 'm': + /* Read in mask directly */ + set_mask = strtoul(optarg, &ep, 0); + if (*ep != '\0') + usage("bad set_mask value"); break; - case 'u': - alt_rate = strtol(optarg, &ep, 0); + case 'a': + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + set_mask |= 1<<i; + } + break; + case 'p': + pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alt_rate value"); + usage("bad PWM value provided"); break; - - case 'm': - set_mask = strtol(optarg, &ep, 0); + case 'r': + alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("bad alternative rate provided"); break; - - case 'v': - print_info = true; + default: break; + } + } - default: - usage(NULL); + if (print_verbose && set_mask > 0) { + warnx("Chose channels: "); + printf(" "); + for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) { + if (set_mask & 1<<i) + printf("%d ", i+1); } + printf("\n"); } - argc -= optind; - argv += optind; /* open for ioctl only */ int fd = open(dev, 0); if (fd < 0) err(1, "can't open %s", dev); - /* change alternate PWM rate */ - if (alt_rate > 0) { - ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + /* get the number of servo channels */ + unsigned servo_count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + if (!strcmp(argv[1], "arm")) { + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); if (ret != OK) - err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); - } + err(1, "PWM_SERVO_SET_ARM_OK"); + /* tell IO that the system is armed (it will output values if safety is off) */ + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + + if (print_verbose) + warnx("Outputs armed"); + exit(0); - /* directly supplied channel mask */ - if (set_mask != -1) { - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask); + } else if (!strcmp(argv[1], "disarm")) { + /* disarm, but do not revoke the SET_ARM_OK flag */ + ret = ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + err(1, "PWM_SERVO_DISARM"); - /* assign alternate rate to channel groups */ - if (alt_channels_set) { - uint32_t mask = 0; + if (print_verbose) + warnx("Outputs disarmed"); + exit(0); - for (unsigned group = 0; group < 32; group++) { - if ((1 << group) & alt_channel_groups) { - uint32_t group_mask; + } else if (!strcmp(argv[1], "rate")) { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); - if (ret != OK) - err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } - mask |= group_mask; - } + /* directly supplied channel mask */ + if (set_mask > 0) { + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask); + if (ret != OK) + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); } - ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); - if (ret != OK) - err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); - } + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; - /* iterate remaining arguments */ - unsigned nchannels = 0; - unsigned channel[8] = {0}; - while (argc--) { - const char *arg = argv[0]; - argv++; - if (!strcmp(arg, "arm")) { - /* tell IO that its ok to disable its safety with the switch */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); - /* tell IO that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); + for (group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask); if (ret != OK) - err(1, "PWM_SERVO_ARM"); - continue; + err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE"); } - if (!strcmp(arg, "disarm")) { - /* disarm, but do not revoke the SET_ARM_OK flag */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); + exit(0); + + } else if (!strcmp(argv[1], "min")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: min PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; + } + + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - continue; + errx(ret, "failed setting min values"); } - unsigned pwm_value = strtol(arg, &ep, 0); - if (*ep == '\0') { - if (nchannels > sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + exit(0); - channel[nchannels] = pwm_value; - nchannels++; + } else if (!strcmp(argv[1], "max")) { - continue; + if (set_mask == 0) { + usage("no channels set"); } - usage("unrecognized option"); - } + if (pwm_value == 0) + usage("no PWM value provided"); - /* print verbose info */ - if (print_info) { - /* get the number of servo channels */ - unsigned count; - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); - if (ret != OK) - err(1, "PWM_SERVO_GET_COUNT"); + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; - /* print current servo values */ - for (unsigned i = 0; i < count; i++) { - servo_position_t spos; + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: max PWM: %d", i+1, pwm_value); + } + pwm_values.channel_count++; + } - ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); - if (ret == OK) { - printf("channel %u: %uus\n", i, spos); - } else { - printf("%u: ERROR\n", i); + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting max values"); + } + exit(0); + + } else if (!strcmp(argv[1], "disarmed")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + warnx("reading disarmed value of zero, disabling disarmed PWM"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("channel %d: disarmed PWM: %d", i+1, pwm_value); } + pwm_values.channel_count++; } - /* print rate groups */ - for (unsigned i = 0; i < count; i++) { - uint32_t group_mask; + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { - ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values); if (ret != OK) - break; - if (group_mask != 0) { - printf("channel group %u: channels", i); - for (unsigned j = 0; j < 32; j++) - if (group_mask & (1 << j)) - printf(" %u", j); - printf("\n"); + errx(ret, "failed setting disarmed values"); + } + exit(0); + + } else if (!strcmp(argv[1], "failsafe")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0}; + + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + pwm_values.values[i] = pwm_value; + if (print_verbose) + warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value); } + pwm_values.channel_count++; } - fflush(stdout); - } - /* perform PWM output */ - if (nchannels) { + if (pwm_values.channel_count == 0) { + usage("no PWM values added"); + } else { + + ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values); + if (ret != OK) + errx(ret, "failed setting failsafe values"); + } + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + if (set_mask == 0) { + usage("no channels set"); + } + if (pwm_value == 0) + usage("no PWM value provided"); + + /* perform PWM output */ /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - err(1, "failed opening console"); + struct pollfd fds; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; warnx("Press CTRL-C or 'c' to abort."); while (1) { - for (int i = 0; i < nchannels; i++) { - ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); - if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", i); + for (unsigned i = 0; i < servo_count; i++) { + if (set_mask & 1<<i) { + ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } } /* abort on user request */ char c; - if (read(console, &c, 1) == 1) { + ret = poll(&fds, 1, 0); + if (ret > 0) { + + read(0, &c, 1); if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); - close(console); exit(0); } } + } + exit(0); + + + } else if (!strcmp(argv[1], "info")) { + + printf("device: %s\n", dev); + + uint32_t info_default_rate; + uint32_t info_alt_rate; + uint32_t info_alt_rate_mask; + + ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate); + if (ret != OK) { + err(1, "PWM_SERVO_GET_UPDATE_RATE"); + } + + ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask); + if (ret != OK) { + err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE"); + } + + struct pwm_output_values failsafe_pwm; + struct pwm_output_values disarmed_pwm; + struct pwm_output_values min_pwm; + struct pwm_output_values max_pwm; + + ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_FAILSAFE_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_DISARMED_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MIN_PWM"); + } + ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm); + if (ret != OK) { + err(1, "PWM_SERVO_GET_MAX_PWM"); + } + + /* print current servo values */ + for (unsigned i = 0; i < servo_count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %u us", i+1, spos); + + if (info_alt_rate_mask & (1<<i)) + printf(" (alternative rate: %d Hz", info_alt_rate); + else + printf(" (default rate: %d Hz", info_default_rate); + + + printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)", + failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]); + printf("\n"); + } else { + printf("%u: ERROR\n", i); + } + } + /* print rate groups */ + for (unsigned i = 0; i < servo_count; i++) { + uint32_t group_mask; - /* rate limit to ~ 20 Hz */ - usleep(50000); + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j+1); + printf("\n"); + } } + exit(0); + } + usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info"); + return 0; +} - exit(0); -}
\ No newline at end of file diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 754d3a0da..6729ce4ae 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -23,6 +23,7 @@ SRCS = test_adc.c \ test_uart_console.c \ test_uart_loopback.c \ test_uart_send.c \ + test_mixer.cpp \ tests_file.c \ tests_main.c \ tests_param.c diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index ba6b86adb..5690997a9 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) { - printf("failed opening /dev/tone_alarm\n"); + printf("failed opening " TONEALARM_DEVICE_PATH "\n"); goto out; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp new file mode 100644 index 000000000..4da86042d --- /dev/null +++ b/src/systemcmds/tests/test_mixer.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * 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 test_mixer.hpp + * + * Mixer load test + */ + +#include <nuttx/config.h> + +#include <sys/types.h> + +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <string.h> +#include <time.h> + +#include <systemlib/err.h> +#include <systemlib/mixer/mixer.h> + +#include "tests.h" + +static int mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control); + +int test_mixer(int argc, char *argv[]) +{ + warnx("testing mixer"); + + char *filename = "/etc/mixers/IO_pass.mix"; + + if (argc > 1) + filename = argv[1]; + + warnx("loading: %s", filename); + + char buf[2048]; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + unsigned loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + /* load the mixer in chunks, like + * in the case of a remote load, + * e.g. on PX4IO. + */ + + unsigned nused = 0; + + const unsigned chunk_size = 64; + + MixerGroup mixer_group(mixer_callback, 0); + + /* load at once test */ + unsigned xx = loaded; + mixer_group.load_from_buf(&buf[0], xx); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; + + unsigned empty_load = 2; + char empty_buf[2]; + empty_buf[0] = ' '; + empty_buf[1] = '\0'; + mixer_group.reset(); + mixer_group.load_from_buf(&empty_buf[0], empty_load); + warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load); + if (empty_load != 0) + return 1; + + /* FIRST mark the mixer as invalid */ + bool mixer_ok = false; + /* THEN actually delete it */ + mixer_group.reset(); + char mixer_text[256]; /* large enough for one mixer */ + unsigned mixer_text_length = 0; + + unsigned transmitted = 0; + + warnx("transmitted: %d, loaded: %d", transmitted, loaded); + + while (transmitted < loaded) { + + unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted; + + /* check for overflow - this would be really fatal */ + if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { + bool mixer_ok = false; + return 1; + } + + /* append mixer text and nul-terminate */ + memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length); + mixer_text_length += text_length; + mixer_text[mixer_text_length] = '\0'; + warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]); + + /* process the text buffer, adding new mixers as their descriptions can be parsed */ + unsigned resid = mixer_text_length; + mixer_group.load_from_buf(&mixer_text[0], resid); + + /* if anything was parsed */ + if (resid != mixer_text_length) { + + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + mixer_ok = true; + } else { + /* not yet reached the end of the mixer, set as not ok */ + mixer_ok = false; + } + + warnx("used %u", mixer_text_length - resid); + + /* copy any leftover text to the base of the buffer for re-use */ + if (resid > 0) + memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); + + mixer_text_length = resid; + } + + transmitted += text_length; + } + + warnx("chunked load: loaded %u mixers", mixer_group.count()); + + if (mixer_group.count() != 8) + return 1; + + /* load multirotor at once test */ + mixer_group.reset(); + + if (argc > 2) + filename = argv[2]; + else + filename = "/etc/mixers/FMU_quad_w.mix"; + + load_mixer_file(filename, &buf[0], sizeof(buf)); + loaded = strlen(buf); + + warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded); + + unsigned mc_loaded = loaded; + mixer_group.load_from_buf(&buf[0], mc_loaded); + warnx("complete buffer load: loaded %u mixers", mixer_group.count()); + if (mixer_group.count() != 8) + return 1; +} + +static int +mixer_callback(uintptr_t handle, + uint8_t control_group, + uint8_t control_index, + float &control) +{ + if (control_group != 0) + return -1; + + control = 0.0f; + + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index c02ea6808..c483108cf 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -76,6 +76,8 @@ * Public Function Prototypes ****************************************************************************/ +__BEGIN_DECLS + extern int test_sensors(int argc, char *argv[]); extern int test_gpio(int argc, char *argv[]); extern int test_hrt(int argc, char *argv[]); @@ -98,5 +100,8 @@ extern int test_jig_voltages(int argc, char *argv[]); extern int test_param(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); +extern int test_mixer(int argc, char *argv[]); + +__END_DECLS #endif /* __APPS_PX4_TESTS_H */ diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 47f480758..7f3a654bf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -38,7 +38,9 @@ */ #include <sys/stat.h> +#include <dirent.h> #include <stdio.h> +#include <stddef.h> #include <unistd.h> #include <fcntl.h> #include <systemlib/err.h> @@ -52,6 +54,224 @@ int test_file(int argc, char *argv[]) { + const unsigned iterations = 100; + const unsigned alignments = 65; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + for (unsigned a = 0; a < alignments; a++) { + + printf("\n"); + warnx("----- alignment test: %u bytes -----", a); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing unaligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); + + } + + fsync(fd); + //perf_end(wperf); + + } + end = hrt_absolute_time(); + + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + /* + * ALIGNED WRITES AND UNALIGNED READS + */ + + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + for (int j = 0; j < chunk_sizes[c]; j++) { + + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); + } + + return 0; +} +#if 0 +int +test_file(int argc, char *argv[]) +{ + const iterations = 1024; + /* check if microSD card is mounted */ struct stat buffer; if (stat("/fs/microsd/", &buffer)) { @@ -67,7 +287,7 @@ test_file(int argc, char *argv[]) memset(buf, 0, sizeof(buf)); start = hrt_absolute_time(); - for (unsigned i = 0; i < 1024; i++) { + for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); write(fd, buf, sizeof(buf)); perf_end(wperf); @@ -78,7 +298,7 @@ test_file(int argc, char *argv[]) unlink("/fs/microsd/testfile"); - warnx("512KiB in %llu microseconds", end - start); + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); perf_print_counter(wperf); perf_free(wperf); @@ -112,3 +332,4 @@ test_file(int argc, char *argv[]) return 0; } +#endif diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 9f8c5c9ea..9eb9c632e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -110,6 +110,7 @@ const struct { {"param", test_param, 0}, {"bson", test_bson, 0}, {"file", test_file, 0}, + {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |