mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merged master
This commit is contained in:
Lorenz Meier
commit
69a183e221
298
Debug/Nuttx.py
Normal file
298
Debug/Nuttx.py
Normal file
@ -0,0 +1,298 @@
|
||||
# GDB/Python functions for dealing with NuttX
|
||||
|
||||
import gdb, gdb.types
|
||||
|
||||
class NX_task(object):
|
||||
"""Reference to a NuttX task and methods for introspecting it"""
|
||||
|
||||
def __init__(self, tcb_ptr):
|
||||
self._tcb = tcb_ptr.dereference()
|
||||
self._group = self._tcb['group'].dereference()
|
||||
self.pid = tcb_ptr['pid']
|
||||
|
||||
@classmethod
|
||||
def for_tcb(cls, tcb):
|
||||
"""return a task with the given TCB pointer"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
pidhash_entry = pidhash_value[i]
|
||||
if pidhash_entry['tcb'] == tcb:
|
||||
return cls(pidhash_entry['tcb'])
|
||||
return None
|
||||
|
||||
@classmethod
|
||||
def for_pid(cls, pid):
|
||||
"""return a task for the given PID"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
pidhash_entry = pidhash_value[i]
|
||||
if pidhash_entry['pid'] == pid:
|
||||
return cls(pidhash_entry['tcb'])
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def pids():
|
||||
"""return a list of all PIDs"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
result = []
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
entry = pidhash_value[i]
|
||||
pid = int(entry['pid'])
|
||||
if pid is not -1:
|
||||
result.append(pid)
|
||||
return result
|
||||
|
||||
@staticmethod
|
||||
def tasks():
|
||||
"""return a list of all tasks"""
|
||||
tasks = []
|
||||
for pid in NX_task.pids():
|
||||
tasks.append(NX_task.for_pid(pid))
|
||||
return tasks
|
||||
|
||||
def _state_is(self, state):
|
||||
"""tests the current state of the task against the passed-in state name"""
|
||||
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
|
||||
if self._tcb['task_state'] == statenames[state]:
|
||||
return True
|
||||
return False
|
||||
|
||||
@property
|
||||
def stack_used(self):
|
||||
"""calculate the stack used by the thread"""
|
||||
if 'stack_used' not in self.__dict__:
|
||||
stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer())
|
||||
if stack_base == 0:
|
||||
self.__dict__['stack_used'] = 0
|
||||
else:
|
||||
stack_limit = self._tcb['adj_stack_size']
|
||||
for offset in range(0, stack_limit):
|
||||
if stack_base[offset] != 0xff:
|
||||
break
|
||||
self.__dict__['stack_used'] = stack_limit - offset
|
||||
return self.__dict__['stack_used']
|
||||
|
||||
@property
|
||||
def name(self):
|
||||
"""return the task's name"""
|
||||
return self._tcb['name'].string()
|
||||
|
||||
@property
|
||||
def state(self):
|
||||
"""return the name of the task's current state"""
|
||||
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
|
||||
for name,value in statenames.iteritems():
|
||||
if value == self._tcb['task_state']:
|
||||
return name
|
||||
return 'UNKNOWN'
|
||||
|
||||
@property
|
||||
def waiting_for(self):
|
||||
"""return a description of what the task is waiting for, if it is waiting"""
|
||||
if self._state_is('TSTATE_WAIT_SEM'):
|
||||
waitsem = self._tcb['waitsem'].dereference()
|
||||
waitsem_holder = waitsem['holder']
|
||||
holder = NX_task.for_tcb(waitsem_holder['htcb'])
|
||||
if holder is not None:
|
||||
return '{}({})'.format(waitsem.address, holder.name)
|
||||
else:
|
||||
return '{}(<bad holder>)'.format(waitsem.address)
|
||||
if self._state_is('TSTATE_WAIT_SIG'):
|
||||
return 'signal'
|
||||
return None
|
||||
|
||||
@property
|
||||
def is_waiting(self):
|
||||
"""tests whether the task is waiting for something"""
|
||||
if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'):
|
||||
return True
|
||||
|
||||
@property
|
||||
def is_runnable(self):
|
||||
"""tests whether the task is runnable"""
|
||||
if (self._state_is('TSTATE_TASK_PENDING') or
|
||||
self._state_is('TSTATE_TASK_READYTORUN') or
|
||||
self._state_is('TSTATE_TASK_RUNNING')):
|
||||
return True
|
||||
return False
|
||||
|
||||
@property
|
||||
def file_descriptors(self):
|
||||
"""return a dictionary of file descriptors and inode pointers"""
|
||||
filelist = self._group['tg_filelist']
|
||||
filearray = filelist['fl_files']
|
||||
result = dict()
|
||||
for i in range(filearray.type.range()[0],filearray.type.range()[1]):
|
||||
inode = long(filearray[i]['f_inode'])
|
||||
if inode != 0:
|
||||
result[i] = inode
|
||||
return result
|
||||
|
||||
@property
|
||||
def registers(self):
|
||||
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
|
||||
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])
|
||||
self.__dict__['registers'] = registers
|
||||
return self.__dict__['registers']
|
||||
|
||||
def __repr__(self):
|
||||
return "<NX_task {}>".format(self.pid)
|
||||
|
||||
def __str__(self):
|
||||
return "{}:{}".format(self.pid, self.name)
|
||||
|
||||
def __format__(self, format_spec):
|
||||
return format_spec.format(
|
||||
pid = self.pid,
|
||||
name = self.name,
|
||||
state = self.state,
|
||||
waiting_for = self.waiting_for,
|
||||
stack_used = self.stack_used,
|
||||
stack_limit = self._tcb['adj_stack_size'],
|
||||
file_descriptors = self.file_descriptors,
|
||||
registers = self.registers
|
||||
)
|
||||
|
||||
class NX_show_task (gdb.Command):
|
||||
"""(NuttX) prints information about a task"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER)
|
||||
|
||||
def invoke(self, arg, from_tty):
|
||||
t = NX_task.for_pid(int(arg))
|
||||
if t is not None:
|
||||
my_fmt = 'PID:{pid} name:{name} state:{state}\n'
|
||||
my_fmt += ' stack used {stack_used} of {stack_limit}\n'
|
||||
if t.is_waiting:
|
||||
my_fmt += ' waiting for {waiting_for}\n'
|
||||
my_fmt += ' open files: {file_descriptors}\n'
|
||||
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(t, my_fmt)
|
||||
|
||||
class NX_show_tasks (gdb.Command):
|
||||
"""(NuttX) prints a list of tasks"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER)
|
||||
|
||||
def invoke(self, args, from_tty):
|
||||
tasks = NX_task.tasks()
|
||||
for t in tasks:
|
||||
print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
|
||||
|
||||
NX_show_task()
|
||||
NX_show_tasks()
|
||||
|
||||
class NX_show_heap (gdb.Command):
|
||||
"""(NuttX) prints the heap"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
|
||||
if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8:
|
||||
self._allocflag = 0x80000000
|
||||
self._allocnodesize = 8
|
||||
else:
|
||||
self._allocflag = 0x8000
|
||||
self._allocnodesize = 4
|
||||
|
||||
def _node_allocated(self, allocnode):
|
||||
if allocnode['preceding'] & self._allocflag:
|
||||
return True
|
||||
return False
|
||||
|
||||
def _node_size(self, allocnode):
|
||||
return allocnode['size'] & ~self._allocflag
|
||||
|
||||
def _print_allocations(self, region_start, region_end):
|
||||
if region_start >= region_end:
|
||||
print 'heap region {} corrupt'.format(hex(region_start))
|
||||
return
|
||||
nodecount = region_end - region_start
|
||||
print 'heap {} - {}'.format(region_start, region_end)
|
||||
cursor = 1
|
||||
while cursor < nodecount:
|
||||
allocnode = region_start[cursor]
|
||||
if self._node_allocated(allocnode):
|
||||
state = ''
|
||||
else:
|
||||
state = '(free)'
|
||||
print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
|
||||
cursor += self._node_size(allocnode) / self._allocnodesize
|
||||
|
||||
def invoke(self, args, from_tty):
|
||||
heap = gdb.lookup_global_symbol('g_mmheap').value()
|
||||
nregions = heap['mm_nregions']
|
||||
region_starts = heap['mm_heapstart']
|
||||
region_ends = heap['mm_heapend']
|
||||
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()
|
||||
|
||||
|
||||
|
||||
|
||||
@ -91,21 +91,61 @@
|
||||
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
|
||||
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
|
||||
|
||||
/**
|
||||
* The PX4IO class.
|
||||
*
|
||||
* Encapsulates PX4FMU to PX4IO communications modeled as file operations.
|
||||
*/
|
||||
class PX4IO : public device::I2C
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor.
|
||||
*
|
||||
* Initialize all class variables.
|
||||
*/
|
||||
PX4IO();
|
||||
/**
|
||||
* Destructor.
|
||||
*
|
||||
* Wait for worker thread to terminate.
|
||||
*/
|
||||
virtual ~PX4IO();
|
||||
|
||||
/**
|
||||
* Initialize the PX4IO class.
|
||||
*
|
||||
* Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
|
||||
*/
|
||||
virtual int init();
|
||||
|
||||
/**
|
||||
* 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
|
||||
* @param[in] cmd the IOCTL command
|
||||
* @param[in] the IOCTL command parameter (optional)
|
||||
*/
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* 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
|
||||
* @param[in] buffer pointer to the data buffer to be written
|
||||
* @param[in] len size in bytes to be written
|
||||
* @return number of bytes written
|
||||
*/
|
||||
virtual ssize_t write(file *filp, const char *buffer, size_t len);
|
||||
|
||||
/**
|
||||
* Set the update rate for actuator outputs from FMU to IO.
|
||||
*
|
||||
* @param rate The rate in Hz actuator outpus are sent to IO.
|
||||
* @param[in] rate The rate in Hz actuator output are sent to IO.
|
||||
* Min 10 Hz, max 400 Hz
|
||||
*/
|
||||
int set_update_rate(int rate);
|
||||
@ -113,16 +153,16 @@ public:
|
||||
/**
|
||||
* Set the battery current scaling and bias
|
||||
*
|
||||
* @param amp_per_volt
|
||||
* @param amp_bias
|
||||
* @param[in] amp_per_volt
|
||||
* @param[in] amp_bias
|
||||
*/
|
||||
void set_battery_current_scaling(float amp_per_volt, float amp_bias);
|
||||
|
||||
/**
|
||||
* Push failsafe values to IO.
|
||||
*
|
||||
* @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
|
||||
* @param len Number of channels, could up to 8
|
||||
* @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
|
||||
* @param[in] len Number of channels, could up to 8
|
||||
*/
|
||||
int set_failsafe_values(const uint16_t *vals, unsigned len);
|
||||
|
||||
@ -142,15 +182,27 @@ public:
|
||||
int set_idle_values(const uint16_t *vals, unsigned len);
|
||||
|
||||
/**
|
||||
* Print the current status of IO
|
||||
*/
|
||||
* Print IO status.
|
||||
*
|
||||
* Print all relevant IO status information
|
||||
*/
|
||||
void print_status();
|
||||
|
||||
/**
|
||||
* 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)
|
||||
{
|
||||
_dsm_vcc_ctl = enable;
|
||||
};
|
||||
|
||||
/**
|
||||
* Get the DSM VCC is controlled by relay one flag
|
||||
*
|
||||
* @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
|
||||
*/
|
||||
inline bool get_dsm_vcc_ctl()
|
||||
{
|
||||
return _dsm_vcc_ctl;
|
||||
@ -158,59 +210,49 @@ public:
|
||||
|
||||
private:
|
||||
// XXX
|
||||
unsigned _max_actuators;
|
||||
unsigned _max_controls;
|
||||
unsigned _max_rc_input;
|
||||
unsigned _max_relays;
|
||||
unsigned _max_transfer;
|
||||
unsigned _max_actuators; ///<Maximum # of actuators supported by PX4IO
|
||||
unsigned _max_controls; ///<Maximum # of controls supported by PX4IO
|
||||
unsigned _max_rc_input; ///<Maximum receiver channels supported by PX4IO
|
||||
unsigned _max_relays; ///<Maximum relays supported by PX4IO
|
||||
unsigned _max_transfer; ///<Maximum number of I2C transfers supported by PX4IO
|
||||
|
||||
unsigned _update_interval; ///< subscription interval limiting send rate
|
||||
unsigned _update_interval; ///<Subscription interval limiting send rate
|
||||
|
||||
volatile int _task; ///< worker task
|
||||
volatile bool _task_should_exit;
|
||||
volatile int _task; ///<worker task id
|
||||
volatile bool _task_should_exit; ///<worker terminate flag
|
||||
|
||||
int _mavlink_fd;
|
||||
int _mavlink_fd; ///<mavlink file descriptor
|
||||
|
||||
perf_counter_t _perf_update;
|
||||
perf_counter_t _perf_update; ///<local performance counter
|
||||
|
||||
/* cached IO state */
|
||||
uint16_t _status;
|
||||
uint16_t _alarms;
|
||||
uint16_t _status; ///<Various IO status flags
|
||||
uint16_t _alarms; ///<Various IO alarms
|
||||
|
||||
/* subscribed topics */
|
||||
int _t_actuators; ///< actuator controls topic
|
||||
int _t_actuators; ///< actuator controls topic
|
||||
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_control_mode;///< vehicle control mode topic
|
||||
int _t_param; ///< parameter update topic
|
||||
|
||||
/* advertised topics */
|
||||
orb_advert_t _to_input_rc; ///< rc inputs from io
|
||||
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_safety; ///< status of safety
|
||||
orb_advert_t _to_outputs; ///< mixed servo outputs topic
|
||||
orb_advert_t _to_battery; ///< battery status / voltage
|
||||
orb_advert_t _to_safety; ///< status of safety
|
||||
|
||||
actuator_outputs_s _outputs; ///< mixed outputs
|
||||
actuator_controls_effective_s _controls_effective; ///< effective controls
|
||||
actuator_outputs_s _outputs; ///<mixed outputs
|
||||
actuator_controls_effective_s _controls_effective; ///<effective controls
|
||||
|
||||
bool _primary_pwm_device; ///< true if we are the default PWM output
|
||||
bool _primary_pwm_device; ///<true if we are the default PWM output
|
||||
|
||||
float _battery_amp_per_volt;
|
||||
float _battery_amp_bias;
|
||||
float _battery_mamphour_total;
|
||||
uint64_t _battery_last_timestamp;
|
||||
float _battery_amp_per_volt; ///<current sensor amps/volt
|
||||
float _battery_amp_bias; ///<current sensor bias
|
||||
float _battery_mamphour_total;///<amp hours consumed so far
|
||||
uint64_t _battery_last_timestamp;///<last amp hour calculation timestamp
|
||||
|
||||
/**
|
||||
* Relay1 is dedicated to controlling DSM receiver power
|
||||
*/
|
||||
|
||||
bool _dsm_vcc_ctl;
|
||||
|
||||
/**
|
||||
* System armed
|
||||
*/
|
||||
|
||||
bool _system_armed;
|
||||
bool _dsm_vcc_ctl; ///<true if relay 1 controls DSM satellite RX power
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
@ -380,8 +422,7 @@ PX4IO::PX4IO() :
|
||||
_battery_amp_bias(0),
|
||||
_battery_mamphour_total(0),
|
||||
_battery_last_timestamp(0),
|
||||
_dsm_vcc_ctl(false),
|
||||
_system_armed(false)
|
||||
_dsm_vcc_ctl(false)
|
||||
{
|
||||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
@ -750,7 +791,7 @@ PX4IO::task_main()
|
||||
// See if bind parameter has been set, and reset it to 0
|
||||
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
|
||||
if (dsm_bind_val) {
|
||||
if (!_system_armed) {
|
||||
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);
|
||||
@ -870,8 +911,6 @@ PX4IO::io_set_arming_state()
|
||||
uint16_t set = 0;
|
||||
uint16_t clear = 0;
|
||||
|
||||
_system_armed = armed.armed;
|
||||
|
||||
if (armed.armed && !armed.lockdown) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
|
||||
} else {
|
||||
@ -1761,7 +1800,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
|
||||
}
|
||||
|
||||
ssize_t
|
||||
PX4IO::write(file *filp, const char *buffer, size_t len)
|
||||
PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
|
||||
/* Make it obvious that file * isn't used here */
|
||||
{
|
||||
unsigned count = len / 2;
|
||||
|
||||
|
||||
@ -48,156 +48,44 @@
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define DEBUG
|
||||
|
||||
#include "px4io.h"
|
||||
|
||||
#define DSM_FRAME_SIZE 16
|
||||
#define DSM_FRAME_CHANNELS 7
|
||||
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
|
||||
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
|
||||
|
||||
static int dsm_fd = -1;
|
||||
|
||||
static hrt_abstime last_rx_time;
|
||||
static hrt_abstime last_frame_time;
|
||||
|
||||
static uint8_t frame[DSM_FRAME_SIZE];
|
||||
|
||||
static unsigned partial_frame_count;
|
||||
static unsigned channel_shift;
|
||||
|
||||
unsigned dsm_frame_drops;
|
||||
|
||||
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
|
||||
static void dsm_guess_format(bool reset);
|
||||
static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
|
||||
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
struct termios t;
|
||||
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
tcgetattr(dsm_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(dsm_fd, TCSANOW, &t);
|
||||
|
||||
/* initialise the decoder */
|
||||
partial_frame_count = 0;
|
||||
last_rx_time = hrt_absolute_time();
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
|
||||
debug("DSM: ready");
|
||||
|
||||
} else {
|
||||
debug("DSM: open failed");
|
||||
}
|
||||
|
||||
return dsm_fd;
|
||||
}
|
||||
|
||||
void
|
||||
dsm_bind(uint16_t cmd, int pulses)
|
||||
{
|
||||
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
|
||||
|
||||
if (dsm_fd < 0)
|
||||
return;
|
||||
|
||||
switch (cmd) {
|
||||
case dsm_bind_power_down:
|
||||
// power down DSM satellite
|
||||
POWER_RELAY1(0);
|
||||
break;
|
||||
case dsm_bind_power_up:
|
||||
POWER_RELAY1(1);
|
||||
dsm_guess_format(true);
|
||||
break;
|
||||
case dsm_bind_set_rx_out:
|
||||
stm32_configgpio(usart1RxAsOutp);
|
||||
break;
|
||||
case dsm_bind_send_pulses:
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
stm32_gpiowrite(usart1RxAsOutp, false);
|
||||
up_udelay(25);
|
||||
stm32_gpiowrite(usart1RxAsOutp, true);
|
||||
up_udelay(25);
|
||||
}
|
||||
break;
|
||||
case dsm_bind_reinit_uart:
|
||||
// Restore USART rx pin
|
||||
stm32_configgpio(GPIO_USART1_RX);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
||||
/*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect frame boundaries by the inter-frame delay.
|
||||
*
|
||||
* The minimum frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* frame transmission time is ~1.4ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a frame.
|
||||
*
|
||||
* In the case where byte(s) are dropped from a frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
*/
|
||||
now = hrt_absolute_time();
|
||||
|
||||
if ((now - last_rx_time) > 5000) {
|
||||
if (partial_frame_count > 0) {
|
||||
dsm_frame_drops++;
|
||||
partial_frame_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current frame.
|
||||
*/
|
||||
ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
return false;
|
||||
|
||||
last_rx_time = now;
|
||||
|
||||
/*
|
||||
* Add bytes to the current frame
|
||||
*/
|
||||
partial_frame_count += ret;
|
||||
|
||||
/*
|
||||
* If we don't have a full frame, return
|
||||
*/
|
||||
if (partial_frame_count < DSM_FRAME_SIZE)
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
partial_frame_count = 0;
|
||||
return dsm_decode(now, values, num_values);
|
||||
}
|
||||
static int dsm_fd = -1; /**<File handle to the DSM UART*/
|
||||
static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/
|
||||
static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/
|
||||
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/
|
||||
static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/
|
||||
static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
|
||||
static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/
|
||||
|
||||
/**
|
||||
* Attempt to decode a single channel raw channel datum
|
||||
*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect dsm frame boundaries by the inter-dsm frame delay.
|
||||
*
|
||||
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* dsm frame transmission time is ~1.4ms.
|
||||
*
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a dsm frame.
|
||||
*
|
||||
* In the case where byte(s) are dropped from a dsm frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
*
|
||||
* Upon receiving a full dsm frame we attempt to decode it
|
||||
*
|
||||
* @param[in] raw 16 bit raw channel value from dsm frame
|
||||
* @param[in] shift position of channel number in raw data
|
||||
* @param[out] channel pointer to returned channel number
|
||||
* @param[out] value pointer to returned channel value
|
||||
* @return true=raw value successfully decoded
|
||||
*/
|
||||
static bool
|
||||
dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
|
||||
{
|
||||
@ -215,6 +103,11 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Attempt to guess if receiving 10 or 11 bit channel values
|
||||
*
|
||||
* @param[in] reset true=reset the 10/11 bit state to unknown
|
||||
*/
|
||||
static void
|
||||
dsm_guess_format(bool reset)
|
||||
{
|
||||
@ -227,14 +120,14 @@ dsm_guess_format(bool reset)
|
||||
cs10 = 0;
|
||||
cs11 = 0;
|
||||
samples = 0;
|
||||
channel_shift = 0;
|
||||
dsm_channel_shift = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
/* scan the channels in the current frame in both 10- and 11-bit mode */
|
||||
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
|
||||
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
|
||||
|
||||
uint8_t *dp = &frame[2 + (2 * i)];
|
||||
uint8_t *dp = &dsm_frame[2 + (2 * i)];
|
||||
uint16_t raw = (dp[0] << 8) | dp[1];
|
||||
unsigned channel, value;
|
||||
|
||||
@ -245,10 +138,10 @@ dsm_guess_format(bool reset)
|
||||
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
|
||||
cs11 |= (1 << channel);
|
||||
|
||||
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
|
||||
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
|
||||
}
|
||||
|
||||
/* wait until we have seen plenty of frames - 2 should normally be enough */
|
||||
/* wait until we have seen plenty of frames - 5 should normally be enough */
|
||||
if (samples++ < 5)
|
||||
return;
|
||||
|
||||
@ -284,13 +177,13 @@ dsm_guess_format(bool reset)
|
||||
}
|
||||
|
||||
if ((votes11 == 1) && (votes10 == 0)) {
|
||||
channel_shift = 11;
|
||||
dsm_channel_shift = 11;
|
||||
debug("DSM: 11-bit format");
|
||||
return;
|
||||
}
|
||||
|
||||
if ((votes10 == 1) && (votes11 == 0)) {
|
||||
channel_shift = 10;
|
||||
dsm_channel_shift = 10;
|
||||
debug("DSM: 10-bit format");
|
||||
return;
|
||||
}
|
||||
@ -300,27 +193,131 @@ dsm_guess_format(bool reset)
|
||||
dsm_guess_format(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialize the DSM receive functionality
|
||||
*
|
||||
* Open the UART for receiving DSM frames and configure it appropriately
|
||||
*
|
||||
* @param[in] device Device name of DSM UART
|
||||
*/
|
||||
int
|
||||
dsm_init(const char *device)
|
||||
{
|
||||
if (dsm_fd < 0)
|
||||
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
|
||||
|
||||
if (dsm_fd >= 0) {
|
||||
|
||||
struct termios t;
|
||||
|
||||
/* 115200bps, no parity, one stop bit */
|
||||
tcgetattr(dsm_fd, &t);
|
||||
cfsetspeed(&t, 115200);
|
||||
t.c_cflag &= ~(CSTOPB | PARENB);
|
||||
tcsetattr(dsm_fd, TCSANOW, &t);
|
||||
|
||||
/* initialise the decoder */
|
||||
dsm_partial_frame_count = 0;
|
||||
dsm_last_rx_time = hrt_absolute_time();
|
||||
|
||||
/* reset the format detector */
|
||||
dsm_guess_format(true);
|
||||
|
||||
debug("DSM: ready");
|
||||
|
||||
} else {
|
||||
|
||||
debug("DSM: open failed");
|
||||
|
||||
}
|
||||
|
||||
return dsm_fd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle DSM satellite receiver bind mode handler
|
||||
*
|
||||
* @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
|
||||
* @param[in] pulses Number of pulses for dsm_bind_send_pulses command
|
||||
*/
|
||||
void
|
||||
dsm_bind(uint16_t cmd, int pulses)
|
||||
{
|
||||
const uint32_t usart1RxAsOutp =
|
||||
GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
|
||||
|
||||
if (dsm_fd < 0)
|
||||
return;
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
case dsm_bind_power_down:
|
||||
|
||||
/*power down DSM satellite*/
|
||||
POWER_RELAY1(0);
|
||||
break;
|
||||
|
||||
case dsm_bind_power_up:
|
||||
|
||||
/*power up DSM satellite*/
|
||||
POWER_RELAY1(1);
|
||||
dsm_guess_format(true);
|
||||
break;
|
||||
|
||||
case dsm_bind_set_rx_out:
|
||||
|
||||
/*Set UART RX pin to active output mode*/
|
||||
stm32_configgpio(usart1RxAsOutp);
|
||||
break;
|
||||
|
||||
case dsm_bind_send_pulses:
|
||||
|
||||
/*Pulse RX pin a number of times*/
|
||||
for (int i = 0; i < pulses; i++) {
|
||||
stm32_gpiowrite(usart1RxAsOutp, false);
|
||||
up_udelay(25);
|
||||
stm32_gpiowrite(usart1RxAsOutp, true);
|
||||
up_udelay(25);
|
||||
}
|
||||
break;
|
||||
|
||||
case dsm_bind_reinit_uart:
|
||||
|
||||
/*Restore USART RX pin to RS232 receive mode*/
|
||||
stm32_configgpio(GPIO_USART1_RX);
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Decode the entire dsm frame (all contained channels)
|
||||
*
|
||||
* @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
|
||||
* @param[out] values pointer to per channel array of decoded values
|
||||
* @param[out] num_values pointer to number of raw channel values returned
|
||||
* @return true=DSM frame successfully decoded, false=no update
|
||||
*/
|
||||
static bool
|
||||
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
|
||||
/*
|
||||
debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
|
||||
frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
|
||||
frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
|
||||
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
|
||||
dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
|
||||
dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
|
||||
*/
|
||||
/*
|
||||
* If we have lost signal for at least a second, reset the
|
||||
* format guessing heuristic.
|
||||
*/
|
||||
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
|
||||
if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
|
||||
dsm_guess_format(true);
|
||||
|
||||
/* we have received something we think is a frame */
|
||||
last_frame_time = frame_time;
|
||||
/* we have received something we think is a dsm_frame */
|
||||
dsm_last_frame_time = frame_time;
|
||||
|
||||
/* if we don't know the frame format, update the guessing state machine */
|
||||
if (channel_shift == 0) {
|
||||
/* if we don't know the dsm_frame format, update the guessing state machine */
|
||||
if (dsm_channel_shift == 0) {
|
||||
dsm_guess_format(false);
|
||||
return false;
|
||||
}
|
||||
@ -332,17 +329,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
* Each channel is a 16-bit unsigned value containing either a 10-
|
||||
* or 11-bit channel value and a 4-bit channel number, shifted
|
||||
* either 10 or 11 bits. The MSB may also be set to indicate the
|
||||
* second frame in variants of the protocol where more than
|
||||
* second dsm_frame in variants of the protocol where more than
|
||||
* seven channels are being transmitted.
|
||||
*/
|
||||
|
||||
for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
|
||||
|
||||
uint8_t *dp = &frame[2 + (2 * i)];
|
||||
uint8_t *dp = &dsm_frame[2 + (2 * i)];
|
||||
uint16_t raw = (dp[0] << 8) | dp[1];
|
||||
unsigned channel, value;
|
||||
|
||||
if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
|
||||
if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
|
||||
continue;
|
||||
|
||||
/* ignore channels out of range */
|
||||
@ -354,7 +351,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
*num_values = channel + 1;
|
||||
|
||||
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
|
||||
if (channel_shift == 11)
|
||||
if (dsm_channel_shift == 11)
|
||||
value /= 2;
|
||||
|
||||
value += 998;
|
||||
@ -385,7 +382,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
values[channel] = value;
|
||||
}
|
||||
|
||||
if (channel_shift == 11)
|
||||
if (dsm_channel_shift == 11)
|
||||
*num_values |= 0x8000;
|
||||
|
||||
/*
|
||||
@ -393,3 +390,70 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
|
||||
*/
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Called periodically to check for input data from the DSM UART
|
||||
*
|
||||
* The DSM* protocol doesn't provide any explicit framing,
|
||||
* so we detect dsm frame boundaries by the inter-dsm frame delay.
|
||||
* The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
|
||||
* dsm frame transmission time is ~1.4ms.
|
||||
* We expect to only be called when bytes arrive for processing,
|
||||
* and if an interval of more than 5ms passes between calls,
|
||||
* the first byte we read will be the first byte of a dsm frame.
|
||||
* In the case where byte(s) are dropped from a dsm frame, this also
|
||||
* provides a degree of protection. Of course, it would be better
|
||||
* if we didn't drop bytes...
|
||||
* 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
|
||||
* @return true=decoded raw channel values updated, false=no update
|
||||
*/
|
||||
bool
|
||||
dsm_input(uint16_t *values, uint16_t *num_values)
|
||||
{
|
||||
ssize_t ret;
|
||||
hrt_abstime now;
|
||||
|
||||
/*
|
||||
*/
|
||||
now = hrt_absolute_time();
|
||||
|
||||
if ((now - dsm_last_rx_time) > 5000) {
|
||||
if (dsm_partial_frame_count > 0) {
|
||||
dsm_frame_drops++;
|
||||
dsm_partial_frame_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Fetch bytes, but no more than we would need to complete
|
||||
* the current dsm frame.
|
||||
*/
|
||||
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
|
||||
|
||||
/* if the read failed for any reason, just give up here */
|
||||
if (ret < 1)
|
||||
return false;
|
||||
|
||||
dsm_last_rx_time = now;
|
||||
|
||||
/*
|
||||
* Add bytes to the current dsm frame
|
||||
*/
|
||||
dsm_partial_frame_count += ret;
|
||||
|
||||
/*
|
||||
* If we don't have a full dsm frame, return
|
||||
*/
|
||||
if (dsm_partial_frame_count < DSM_FRAME_SIZE)
|
||||
return false;
|
||||
|
||||
/*
|
||||
* Great, it looks like we might have a dsm frame. Go ahead and
|
||||
* decode it.
|
||||
*/
|
||||
dsm_partial_frame_count = 0;
|
||||
return dsm_decode(now, values, num_values);
|
||||
}
|
||||
|
||||
@ -187,7 +187,7 @@ extern void controls_init(void);
|
||||
extern void controls_tick(void);
|
||||
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 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);
|
||||
|
||||
@ -195,9 +195,9 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
|
||||
extern volatile uint8_t debug_level;
|
||||
|
||||
/* send a debug message to the console */
|
||||
extern void isr_debug(uint8_t level, const char *fmt, ...);
|
||||
extern void isr_debug(uint8_t level, const char *fmt, ...);
|
||||
|
||||
#ifdef CONFIG_STM32_I2C1
|
||||
void i2c_dump(void);
|
||||
void i2c_reset(void);
|
||||
void i2c_dump(void);
|
||||
void i2c_reset(void);
|
||||
#endif
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user