diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 18:51:25 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-19 18:51:25 +0200 |
commit | 69a183e221a6ae2fc1f9677b0193d11e43d46d72 (patch) | |
tree | df7638e76652bb18613c1290d00d994c9e56f01d | |
parent | 449dc78ae69e888d986185f120aa8c6549ef5c2b (diff) | |
parent | d90345a16619a6a056ca9158961db36787d97678 (diff) | |
download | px4-firmware-69a183e221a6ae2fc1f9677b0193d11e43d46d72.tar.gz px4-firmware-69a183e221a6ae2fc1f9677b0193d11e43d46d72.tar.bz2 px4-firmware-69a183e221a6ae2fc1f9677b0193d11e43d46d72.zip |
Merged master
-rw-r--r-- | Debug/Nuttx.py | 298 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 140 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 398 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 8 |
4 files changed, 623 insertions, 221 deletions
diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py new file mode 100644 index 000000000..b0864a229 --- /dev/null +++ b/Debug/Nuttx.py @@ -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() + + + + diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 65e8fa4b6..332348925 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index b2c0db425..745cdfa40 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -48,156 +48,44 @@ #include <drivers/drv_hrt.h> -#define DEBUG - #include "px4io.h" -#define DSM_FRAME_SIZE 16 -#define DSM_FRAME_CHANNELS 7 - -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); +#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ +#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ - 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); +} diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a020f8704..587ca4e30 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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 |