diff --git a/Debug/NuttX b/Debug/NuttX index d34e9f5b4b..4b9f4b5a1b 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -78,7 +78,7 @@ end ################################################################################ define showfiles - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file) printf "%d files\n", $nfiles set $index = 0 @@ -102,7 +102,7 @@ end ################################################################################ define _showtask_oneline - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf " %p %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name end @@ -139,7 +139,7 @@ end # Print task registers for a NuttX v7em target with FPU enabled. # define _showtaskregs_v7em - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 set $regs = (uint32_t *)&($task->xcp.regs[0]) printf " r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5] @@ -162,7 +162,7 @@ end define _showsemaphore printf "count %d ", $arg0->semcount if $arg0->holder.htcb != 0 - set $_task = (struct _TCB *)$arg0->holder.htcb + set $_task = (struct tcb_s *)$arg0->holder.htcb printf "held by %s", $_task->name end printf "\n" @@ -172,7 +172,7 @@ end # Print information about a task's stack usage # define showtaskstack - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 if $task == &g_idletcb printf "can't measure idle stack\n" @@ -189,7 +189,7 @@ end # Print details of a task # define showtask - set $task = (struct _TCB *)$arg0 + set $task = (struct tcb_s *)$arg0 printf "%p %.2d ", $task, $task->pid _showtaskstate $task @@ -204,7 +204,7 @@ define showtask if $task->task_state != TSTATE_TASK_RUNNING _showtaskregs_v7em $task else - _showcurrentregs_v7em + _showtaskregs_v7em $task end # XXX print registers here @@ -247,8 +247,10 @@ define showtasks _showtasklist &g_pendingtasks printf "RUNNABLE\n" _showtasklist &g_readytorun - printf "WAITING\n" + printf "WAITING for Semaphore\n" _showtasklist &g_waitingforsemaphore + printf "WAITING for Signal\n" + _showtasklist &g_waitingforsignal printf "INACTIVE\n" _showtasklist &g_inactivetasks end @@ -257,3 +259,23 @@ document showtasks . showtasks . Print a list of all tasks in the system, separated into their respective queues. end + +define my_mem + + set $start = $arg0 + set $end = $arg1 + set $cursor = $start + + if $start < $end + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor + 4 + printf "0x%x of 0x%x\n",$cursor,$end + end + else + while $cursor != $end + set *$cursor = 0x0000 + set $cursor = $cursor - 4 + end + end +end \ No newline at end of file diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index 7cc21b99f4..cf86dd668f 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -59,30 +59,42 @@ class NX_register_set(object): 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')) + self.regs['R0'] = self.mon_reg_call('r0') + self.regs['R1'] = self.mon_reg_call('r1') + self.regs['R2'] = self.mon_reg_call('r2') + self.regs['R3'] = self.mon_reg_call('r3') + self.regs['R4'] = self.mon_reg_call('r4') + self.regs['R5'] = self.mon_reg_call('r5') + self.regs['R6'] = self.mon_reg_call('r6') + self.regs['R7'] = self.mon_reg_call('r7') + self.regs['R8'] = self.mon_reg_call('r8') + self.regs['R9'] = self.mon_reg_call('r9') + self.regs['R10'] = self.mon_reg_call('r10') + self.regs['R11'] = self.mon_reg_call('r11') + self.regs['R12'] = self.mon_reg_call('r12') + self.regs['R13'] = self.mon_reg_call('r13') + self.regs['SP'] = self.mon_reg_call('sp') + self.regs['R14'] = self.mon_reg_call('r14') + self.regs['LR'] = self.mon_reg_call('lr') + self.regs['R15'] = self.mon_reg_call('r15') + self.regs['PC'] = self.mon_reg_call('pc') + self.regs['XPSR'] = self.mon_reg_call('xPSR') else: for key in self.v7em_regmap.keys(): self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + def mon_reg_call(self,register): + """ + register is the register as a string e.g. 'pc' + return integer containing the value of the register + """ + str_to_eval = "mon reg "+register + resp = gdb.execute(str_to_eval,to_string = True) + content = resp.split()[-1]; + try: + return int(content,16) + except: + return 0 @classmethod def with_xcpt_regs(cls, xcpt_regs): @@ -172,7 +184,7 @@ class NX_task(object): self.__dict__['stack_used'] = 0 else: stack_limit = self._tcb['adj_stack_size'] - for offset in range(0, stack_limit): + for offset in range(0, int(stack_limit)): if stack_base[offset] != 0xff: break self.__dict__['stack_used'] = stack_limit - offset @@ -187,7 +199,7 @@ class NX_task(object): 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(): + for name,value in statenames.items(): if value == self._tcb['task_state']: return name return 'UNKNOWN' @@ -196,16 +208,19 @@ class NX_task(object): 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 '{}()'.format(waitsem.address) + try: + 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 '{}()'.format(waitsem.address) + except: + return 'EXCEPTION' if self._state_is('TSTATE_WAIT_SIG'): return 'signal' - return None + return "" @property def is_waiting(self): @@ -229,7 +244,7 @@ class NX_task(object): filearray = filelist['fl_files'] result = dict() for i in range(filearray.type.range()[0],filearray.type.range()[1]): - inode = long(filearray[i]['f_inode']) + inode = int(filearray[i]['f_inode']) if inode != 0: result[i] = inode return result @@ -253,7 +268,18 @@ class NX_task(object): def __str__(self): return "{}:{}".format(self.pid, self.name) - + + def showoff(self): + print("-------") + print(self.pid,end = ", ") + print(self.name,end = ", ") + print(self.state,end = ", ") + print(self.waiting_for,end = ", ") + print(self.stack_used,end = ", ") + print(self._tcb['adj_stack_size'],end = ", ") + print(self.file_descriptors) + print(self.registers) + def __format__(self, format_spec): return format_spec.format( pid = self.pid, @@ -265,7 +291,7 @@ class NX_task(object): file_descriptors = self.file_descriptors, registers = self.registers ) - + class NX_show_task (gdb.Command): """(NuttX) prints information about a task""" @@ -285,7 +311,7 @@ class NX_show_task (gdb.Command): 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) + print(format(t, my_fmt)) class NX_show_tasks (gdb.Command): """(NuttX) prints a list of tasks""" @@ -295,8 +321,10 @@ class NX_show_tasks (gdb.Command): def invoke(self, args, from_tty): tasks = NX_task.tasks() + print ('Number of tasks: ' + str(len(tasks))) for t in tasks: - print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + #t.showoff() + print(format(t, 'Task: {pid} {name} {state} {stack_used}/{stack_limit}')) NX_show_task() NX_show_tasks() @@ -306,15 +334,15 @@ class NX_show_heap (gdb.Command): def __init__(self): super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) - struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') - preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof - if preceding_size == 2: + struct_mm_allocnode_s = gdb.lookup_type('struct mm_allocnode_s') + preceding_size = struct_mm_allocnode_s['preceding'].type.sizeof + if preceding_size == 2: self._allocflag = 0x8000 - elif preceding_size == 4: + elif preceding_size == 4: self._allocflag = 0x80000000 - else: - raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) - self._allocnodesize = struct_mm_allocnode_s.sizeof + else: + raise gdb.GdbError('invalid mm_allocnode_s.preceding size %u' % preceding_size) + self._allocnodesize = struct_mm_allocnode_s.sizeof def _node_allocated(self, allocnode): if allocnode['preceding'] & self._allocflag: @@ -328,7 +356,7 @@ class NX_show_heap (gdb.Command): if region_start >= region_end: raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start - print 'heap {} - {}'.format(region_start, region_end) + print ('heap {} - {}'.format(region_start, region_end)) cursor = 1 while cursor < nodecount: allocnode = region_start[cursor] @@ -336,8 +364,8 @@ class NX_show_heap (gdb.Command): state = '' else: state = '(free)' - print ' {} {} {}'.format(allocnode.address + self._allocnodesize, - self._node_size(allocnode), state) + print( ' {} {} {}'.format(allocnode.address + self._allocnodesize, + self._node_size(allocnode), state)) cursor += self._node_size(allocnode) / self._allocnodesize def invoke(self, args, from_tty): @@ -345,7 +373,7 @@ 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]) @@ -370,6 +398,317 @@ class NX_show_interrupted_thread (gdb.Command): 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) + print (format(registers, my_fmt)) NX_show_interrupted_thread() + +class NX_check_tcb(gdb.Command): + """ check the tcb of a task from a address """ + def __init__(self): + super(NX_check_tcb,self).__init__('show tcb', gdb.COMMAND_USER) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + print("tcb int: ",int(args)) + print(tasks[int(args)]._tcb) + a =tasks[int(args)]._tcb['xcp']['regs'] + print("relevant registers:") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) +NX_check_tcb() + +class NX_tcb(object): + def __init__(self): + pass + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + print(li) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + +class NX_check_stack_order(gdb.Command): + """ Check the Stack order corresponding to the tasks """ + + def __init__(self): + super(NX_check_stack_order,self).__init__('show check_stack', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def getTCB(self): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tcb_list = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tcb_list = tcb_list + self.find_tcb_list(cursor) + return tcb_list + + def getSPfromTask(self,tcb): + regmap = NX_register_set.v7em_regmap + a =tcb['xcp']['regs'] + return int(a[regmap['SP']]) + + def find_closest(self,list,val): + tmp_list = [abs(i-val) for i in list] + tmp_min = min(tmp_list) + idx = tmp_list.index(tmp_min) + return idx,list[idx] + + def find_next_stack(self,address,_dict_in): + add_list = [] + name_list = [] + for key in _dict_in.keys(): + for i in range(3): + if _dict_in[key][i] < address: + add_list.append(_dict_in[key][i]) + if i == 2: # the last one is the processes stack pointer + name_list.append(self.check_name(key)+"_SP") + else: + name_list.append(self.check_name(key)) + + idx,new_address = self.find_closest(add_list,address) + return new_address,name_list[idx] + + def check_name(self,name): + if isinstance(name,(list)): + name = name[0]; + idx = name.find("\\") + newname = name[:idx] + + return newname + + def invoke(self,args,sth): + tcb = self.getTCB(); + stackadresses={}; + for t in tcb: + p = []; + #print(t.name,t._tcb['stack_alloc_ptr']) + p.append(int(t['stack_alloc_ptr'])) + p.append(int(t['adj_stack_ptr'])) + p.append(self.getSPfromTask(t)) + stackadresses[str(t['name'])] = p; + address = int("0x30000000",0) + print("stack address : process") + for i in range(len(stackadresses)*3): + address,name = self.find_next_stack(address,stackadresses) + print(hex(address),": ",name) + +NX_check_stack_order() + +class NX_run_debug_util(gdb.Command): + """ show the registers of a task corresponding to a tcb address""" + def __init__(self): + super(NX_run_debug_util,self).__init__('show regs', gdb.COMMAND_USER) + + def printRegisters(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + print("relevant registers in ",task.name,":") + for reg in regmap: + hex_addr= hex(int(a[regmap[reg]])) + eval_string = 'info line *'+str(hex_addr) + print(reg,": ",hex_addr,) + + def getPCfromTask(self,task): + regmap = NX_register_set.v7em_regmap + a =task._tcb['xcp']['regs'] + return hex(int(a[regmap['PC']])) + + def invoke(self,args,sth): + tasks = NX_task.tasks() + if args == '': + for t in tasks: + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + else: + tcb_nr = int(args); + print("tcb_nr = ",tcb_nr) + t = tasks[tcb_nr] + self.printRegisters(t) + eval_str = "list *"+str(self.getPCfromTask(t)) + print("this is the location in code where the current threads $pc is:") + gdb.execute(eval_str) + +NX_run_debug_util() + + +class NX_search_tcb(gdb.Command): + """ shot PID's of all running tasks """ + + def __init__(self): + super(NX_search_tcb,self).__init__('show alltcb', gdb.COMMAND_USER) + + def is_in(self,arg,list): + for i in list: + if arg == i: + return True; + return False + + def find_tcb_list(self,dq_entry_t): + tcb_list = [] + tcb_ptr = dq_entry_t.cast(gdb.lookup_type('struct tcb_s').pointer()) + first_tcb = tcb_ptr.dereference() + tcb_list.append(first_tcb); + next_tcb = first_tcb['flink'].dereference() + while not self.is_in(int(next_tcb['pid']),[int(t['pid']) for t in tcb_list]): + tcb_list.append(next_tcb); + old_tcb = next_tcb; + next_tcb = old_tcb['flink'].dereference() + + return [t for t in tcb_list if int(t['pid'])<2000] + + def invoke(self,args,sth): + list_of_listsnames = ['g_pendingtasks','g_readytorun','g_waitingforsemaphore','g_waitingforsignal','g_inactivetasks'] + tasks = []; + for l in list_of_listsnames: + li = gdb.lookup_global_symbol(l) + cursor = li.value()['head'] + tasks = tasks + self.find_tcb_list(cursor) + + # filter for tasks that are listed twice + tasks_filt = {} + for t in tasks: + pid = int(t['pid']); + if not pid in tasks_filt.keys(): + tasks_filt[pid] = t['name']; + print('{num_t} Tasks found:'.format(num_t = len(tasks_filt))) + for pid in tasks_filt.keys(): + print("PID: ",pid," ",tasks_filt[pid]) + +NX_search_tcb() + + +class NX_my_bt(gdb.Command): + """ 'fake' backtrace: backtrace the stack of a process and check every suspicious address for the list + arg: tcb_address$ + (can easily be found by typing 'showtask'). + """ + + def __init__(self): + super(NX_my_bt,self).__init__('show mybt', gdb.COMMAND_USER) + + def readmem(self,addr): + ''' + read memory at addr and return nr + ''' + str_to_eval = "x/x "+hex(addr) + resp = gdb.execute(str_to_eval,to_string = True) + idx = resp.find('\t') + return int(resp[idx:],16) + + def is_in_bounds(self,val): + lower_bound = int("08004000",16) + upper_bound = int("080ae0c0",16); + #print(lower_bound," ",val," ",upper_bound) + if val>lower_bound and val0: + name = words[3][idx+1:]; + path = words[3][:idx]; + else: + name = words[3]; + path = ""; + block = gdb.block_for_pc(addr) + func = block.function + if str(func) == "None": + func = block.superblock.function + + if valid: + print("Line: ",line," in ",path,"/",name,"in ",func) + return name,path,line,func + + + + + def invoke(self,args,sth): + addr_dec = int(args[2:],16) + _tcb = self.get_tcb_from_address(addr_dec) + print("found task with PID: ",_tcb["pid"]) + up_stack = int(_tcb['adj_stack_ptr']) + curr_sp = int(_tcb['xcp']['regs'][0]) #curr stack pointer + other_sp = int(_tcb['xcp']['regs'][8]) # other stack pointer + stacksize = int(_tcb['adj_stack_size']) # other stack pointer + + print("tasks current SP = ",hex(curr_sp),"stack max ptr is at ",hex(up_stack)) + + if curr_sp == up_stack: + sp = other_sp + else: + sp = curr_sp; + + while(sp < up_stack): + mem = self.readmem(sp) + #print(hex(sp)," : ",hex(mem)) + if self.is_in_bounds(mem): + # this is a potential instruction ptr + stack_percentage = (up_stack-sp)/stacksize + name,path,line,func = self.print_instruction_at(mem,stack_percentage) + sp = sp + 4; # jump up one word + +NX_my_bt() diff --git a/Documentation/versionfilter.sh b/Documentation/versionfilter.sh old mode 100644 new mode 100755 diff --git a/NuttX b/NuttX index 5ee4b2b2c2..ae4b05e2c5 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 5ee4b2b2c26bbc35d1669840f0676e8aa383b984 +Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil index d114fe21a2..4d6d350b8a 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -1,14 +1,10 @@ #!nsh # -# HILStar / X-Plane -# -# Lorenz Meier +# HILStar +# # sh /etc/init.d/rc.fw_defaults -echo "X Plane HIL starting.." - set HIL yes - set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index b1aa8c00b3..c8379e3a1a 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -2,7 +2,7 @@ # # Team Blacksheep Discovery Quadcopter # -# Anton Babushkin , Simon Wilks , Thomas Gubler +# Anton Babushkin , Simon Wilks # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index 3f47390c15..0b422de7e2 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -2,7 +2,7 @@ # # 3DR Iris Quadcopter # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 6179855f6a..a621de7ce8 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -2,8 +2,7 @@ # # Steadidrone QU4D # -# Thomas Gubler -# Lorenz Meier +# Thomas Gubler # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil index 7a7a9542c0..1c4f6803b6 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter X # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index c47500c7ac..0cbdd75bed 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -2,7 +2,7 @@ # # HIL Quadcopter + # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil index 4e3e183262..fb440d2fc7 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -2,13 +2,11 @@ # # HIL Rascal 110 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." - set HIL yes set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil index 941f5664ad..00b97d675c 100644 --- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil +++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil @@ -2,7 +2,7 @@ # # HIL Malolo 1 (Flightgear) # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index daa04a4de8..e4d96fbd56 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,12 +2,12 @@ # # Generic 10" Hexa coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_cox -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 8703f5f2fe..f820251ad3 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -2,7 +2,7 @@ # # Generic 10" Octo coaxial geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 83c470234c..0f0cb3a89f 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -3,3 +3,6 @@ sh /etc/init.d/rc.fw_defaults set MIXER FMU_Q +# Provide ESC a constant 1000 us pulse while disarmed +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 53c48d8aac..a7749cba6e 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,7 +2,7 @@ # # Phantom FPV Flying Wing # -# Simon Wilks , Thomas Gubler +# Simon Wilks # sh /etc/init.d/rc.fw_defaults @@ -21,8 +21,6 @@ then param set FW_PR_P 0.03 param set FW_P_LIM_MAX 50 param set FW_P_LIM_MIN -50 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 param set FW_RR_I 0.02 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 7d0dc5bff3..26c7c95e67 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -2,7 +2,7 @@ # # Skywalker X5 Flying Wing # -# Thomas Gubler , Julian Oes +# Thomas Gubler , Julian Oes # sh /etc/init.d/rc.fw_defaults @@ -19,10 +19,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 1 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index f4dedef15c..919eefb4a8 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -2,7 +2,7 @@ # # Wing Wing (aka Z-84) Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 index f4bd18269d..4a76ba6ebd 100644 --- a/ROMFS/px4fmu_common/init.d/3034_fx79 +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -2,7 +2,7 @@ # # FX-79 Buffalo Flying Wing # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index f3b0e84183..0f5f5502aa 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -2,7 +2,7 @@ # # Viper # -# Simon Wilks +# Simon Wilks # sh /etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 9a21504035..9bfd9d9ed5 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -1,8 +1,8 @@ #!nsh # -# TBS Caipirinha Flying Wing +# TBS Caipirinha # -# Thomas Gubler +# Thomas Gubler # sh /etc/init.d/rc.fw_defaults @@ -22,10 +22,6 @@ then param set FW_PR_I 0 param set FW_PR_IMAX 0.2 param set FW_PR_P 0.03 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 param set FW_P_ROLLFF 0 param set FW_RR_FF 0.3 param set FW_RR_I 0 diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 06c54a41db..8fe8961c5c 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -2,7 +2,7 @@ # # Generic 10" Quad X geometry # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index e6007db0e7..0488e3928e 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -3,9 +3,6 @@ # ARDrone # -echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board" - -# Just use the default multicopter settings. sh /etc/init.d/rc.mc_defaults # diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 282ab620da..f0cc052075 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F330 Quadcopter +# DJI Flame Wheel F330 # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 517b4aa86a..1ca716a6b3 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,8 +1,8 @@ #!nsh # -# DJI Flame Wheel F450 Quadcopter +# DJI Flame Wheel F450 # -# Lorenz Meier +# Lorenz Meier # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 8c5a4fbf2d..5c4a6497a0 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -2,7 +2,7 @@ # # F450-sized quadrotor with CAN # -# Lorenz Meier +# Pavel Kirienko # sh /etc/init.d/4001_quad_x diff --git a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb index 99ffd73a5f..9fe310ddee 100644 --- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb +++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb @@ -3,7 +3,7 @@ # Hobbyking Micro Integrated PCB Quadcopter # with SimonK ESC firmware and Mystery A1510 motors # -# Thomas Gubler +# Thomas Gubler # echo "HK Micro PCB Quad" diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 1fb25e5d88..5512aa738a 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -2,7 +2,7 @@ # # Generic 10" Quad + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 34fc6523fe..1043ad8adb 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -2,12 +2,12 @@ # # Generic 10" Hexa X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_x -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 235e376a6e..84ab88883c 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -2,12 +2,12 @@ # # Generic 10" Hexa + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults set MIXER FMU_hexa_+ -# We only can run one channel group with one rate, so set all 8 channels +# Need to set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 769217dc78..74e304cd96 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -2,7 +2,7 @@ # # Generic 10" Octo X geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 28b074a545..f7c06c6c80 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -2,7 +2,7 @@ # # Generic 10" Octo + geometry # -# Anton Babushkin +# Anton Babushkin # sh /etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 78778d8066..496a52c5fa 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -1,5 +1,4 @@ # -# Check if auto-setup from one of the standard scripts is wanted # SYS_AUTOSTART = 0 means no autostart (default) # # AUTOSTART PARTITION: @@ -18,7 +17,7 @@ # 12000 .. 12999 Octo Cox # -# Simulation setups +# Simulation # if param compare SYS_AUTOSTART 901 @@ -53,7 +52,7 @@ then fi # -# Standard plane +# Plane # if param compare SYS_AUTOSTART 2100 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e44cd0953d..41e0b149b4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -10,7 +10,7 @@ then # set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix - #Use the mixer file from the SD-card if it exists + # Use the mixer file from the SD-card if it exists if [ -f $MIXERSD ] then set MIXER_FILE $MIXERSD @@ -54,7 +54,6 @@ then # if [ $PWM_RATE != none ] then - echo "[init] Set PWM rate: $PWM_RATE" pwm rate -c $PWM_OUTPUTS -r $PWM_RATE fi @@ -63,17 +62,14 @@ then # if [ $PWM_DISARMED != none ] then - echo "[init] Set PWM disarmed: $PWM_DISARMED" pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED fi if [ $PWM_MIN != none ] then - echo "[init] Set PWM min: $PWM_MIN" pwm min -c $PWM_OUTPUTS -p $PWM_MIN fi if [ $PWM_MAX != none ] then - echo "[init] Set PWM max: $PWM_MAX" pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.jig b/ROMFS/px4fmu_common/init.d/rc.jig deleted file mode 100644 index e2b5d8f30d..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.jig +++ /dev/null @@ -1,10 +0,0 @@ -#!nsh -# -# Test jig startup script -# - -echo "[testing] doing production test.." - -tests jig - -echo "[testing] testing done" diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 739df7ac0b..fbac50cf71 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -54,7 +54,6 @@ then fi fi -# Start airspeed sensors if meas_airspeed start then echo "[init] Using MEAS airspeed sensor" @@ -68,16 +67,12 @@ else fi fi -# Check for flow sensor if px4flow start then fi # -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. +# Start sensors -> preflight_check # if sensors start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea04ece34d..353f448776 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -137,9 +137,7 @@ then # if param compare SYS_AUTOCONFIG 1 then - # We can't be sure the defaults haven't changed, so - # if someone requests a re-configuration, we do it - # cleanly from scratch (except autostart / autoconfig) + # Wipe out params param reset_nostart set DO_AUTOCONFIG yes else @@ -202,12 +200,10 @@ then if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK" echo "PX4IO CRC OK" >> $LOG_FILE set IO_PRESENT yes else - echo "[init] Trying to update" echo "PX4IO Trying to update" >> $LOG_FILE tone_alarm MLL32CP8MB @@ -217,18 +213,15 @@ then usleep 500000 if px4io checkcrc $IO_FILE then - echo "[init] PX4IO CRC OK, update successful" echo "PX4IO CRC OK after updating" >> $LOG_FILE tone_alarm MLL8CDE set IO_PRESENT yes else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi else - echo "[init] ERROR: PX4IO update failed" echo "PX4IO update failed" >> $LOG_FILE tone_alarm $TUNE_OUT_ERROR fi @@ -281,16 +274,12 @@ then fi fi - # - # Start the datamanager (and do not abort boot if it fails) - # + # waypoint storage if dataman start then fi - # - # Start the Commander (needs to be this early for in-air-restarts) - # + # Needs to be this early for in-air-restarts commander start # @@ -312,7 +301,6 @@ then if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ] then - echo "[init] Use PX4IO PWM as primary output" if px4io start then echo "[init] PX4IO started" @@ -325,7 +313,6 @@ then if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then - echo "[init] Use FMU as primary output" if fmu mode_$FMU_MODE then echo "[init] FMU mode_$FMU_MODE started" @@ -349,7 +336,6 @@ then if [ $OUTPUT_MODE == mkblctrl ] then - echo "[init] Use MKBLCTRL as primary output" set MKBLCTRL_ARG "" if [ $MKBLCTRL_MODE == x ] then @@ -372,7 +358,6 @@ then if [ $OUTPUT_MODE == hil ] then - echo "[init] Use HIL as primary output" if hil mode_port2_pwm8 then echo "[init] HIL output started" @@ -391,7 +376,6 @@ then then if px4io start then - echo "[init] PX4IO started" sh /etc/init.d/rc.io else echo "[init] ERROR: PX4IO start failed" @@ -424,9 +408,6 @@ then fi fi - # - # MAVLink - # if [ $MAVLINK_FLAGS == default ] then # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s @@ -454,15 +435,10 @@ then # Sensors, Logging, GPS # sh /etc/init.d/rc.sensors - - # - # Start logging in all modes, including HIL - # sh /etc/init.d/rc.logging if [ $GPS == yes ] then - echo "[init] Start GPS" if [ $GPS_FAKE == yes ] then echo "[init] Faking GPS" diff --git a/Tools/px_generate_xml.sh b/Tools/px_generate_xml.sh old mode 100644 new mode 100755 diff --git a/Tools/px_mkfw.py b/Tools/px_mkfw.py index b598a65a1a..c2da8a2038 100755 --- a/Tools/px_mkfw.py +++ b/Tools/px_mkfw.py @@ -73,6 +73,7 @@ parser.add_argument("--version", action="store", help="set a version string") parser.add_argument("--summary", action="store", help="set a brief description") parser.add_argument("--description", action="store", help="set a longer description") parser.add_argument("--git_identity", action="store", help="the working directory to check for git identity") +parser.add_argument("--parameter_xml", action="store", help="the parameters.xml file") parser.add_argument("--image", action="store", help="the firmware image") args = parser.parse_args() @@ -101,6 +102,10 @@ if args.git_identity != None: p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout desc['git_identity'] = str(p.read().strip()) p.close() +if args.parameter_xml != None: + f = open(args.parameter_xml, "rb") + bytes = f.read() + desc['parameter_xml'] = base64.b64encode(zlib.compress(bytes,9)).decode('utf-8') if args.image != None: f = open(args.image, "rb") bytes = f.read() diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh old mode 100644 new mode 100755 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d4e4612262..3a4540ac06 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -458,7 +458,7 @@ if os.path.exists("/usr/sbin/ModemManager"): # Load the firmware file fw = firmware(args.firmware) -print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))) +print("Loaded firmware for %x,%x, size: %d bytes, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'))) print("If the board does not respond within 1-2 seconds, unplug and re-plug the USB connector.") # Spin waiting for a device to show up diff --git a/Tools/upload.sh b/Tools/upload.sh index 17d87fe610..c0dd65eba1 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -10,17 +10,17 @@ SYSTYPE=`uname -s` # # XXX The uploader should be smarter than this. # -if [ $SYSTYPE=Darwin ]; +if [ $SYSTYPE = "Darwin" ]; then SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi -if [ $SYSTYPE=Linux ]; +if [ $SYSTYPE = "Linux" ]; then SERIAL_PORTS="/dev/serial/by-id/usb-3D_Robotics*" fi -if [ $SYSTYPE="" ]; +if [ $SYSTYPE = "" ]; then 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" fi diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9fe16fbb6c..18be47065d 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,6 +24,8 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 +#MODULES += drivers/ll40ls +MODULES += drivers/trone #MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil @@ -132,6 +134,9 @@ MODULES += lib/launchdetection # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # 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 d17dff5bb0..df6b6d0c5b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,27 +27,22 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/sf0x +# MODULES += drivers/sf0x MODULES += drivers/ll40ls +# MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors -MODULES += drivers/blinkm +# MODULES += drivers/blinkm MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl -MODULES += drivers/pca8574 MODULES += drivers/px4flow - -# Needs to be burned to the ground and re-written; for now, -# just don't build it. -#MODULES += drivers/mkblctrl - # # System commands # @@ -61,7 +56,6 @@ MODULES += systemcmds/pwm MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top -MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd @@ -81,10 +75,8 @@ MODULES += modules/uavcan # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/attitude_estimator_so3 MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator # # Vehicle Control @@ -100,12 +92,6 @@ MODULES += modules/mc_pos_control # MODULES += modules/sdlog2 -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - # # Library modules # @@ -139,7 +125,7 @@ MODULES += modules/bottle_drop #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -MODULES += examples/px4_simple_app +#MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon @@ -156,6 +142,9 @@ MODULES += examples/px4_simple_app # Hardware test #MODULES += examples/hwtest +# Generate parameter XML file +GEN_PARAM_XML = 1 + # # Transitional support - add commands from the NuttX export archive. # diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 2f4d9d6a47..6f54b960c1 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -49,6 +49,13 @@ MODULES += lib/mathlib MODULES += lib/mathlib/math/filter MODULES += lib/conversion +# +# Modules to test-build, but not useful for test environment +# +MODULES += modules/attitude_estimator_so3 +MODULES += drivers/pca8574 +MODULES += examples/flow_position_estimator + # # Libraries # diff --git a/makefiles/firmware.mk b/makefiles/firmware.mk index 60602e76fe..21e8739aa1 100644 --- a/makefiles/firmware.mk +++ b/makefiles/firmware.mk @@ -467,6 +467,7 @@ endif PRODUCT_BUNDLE = $(WORK_DIR)firmware.px4 PRODUCT_BIN = $(WORK_DIR)firmware.bin PRODUCT_ELF = $(WORK_DIR)firmware.elf +PRODUCT_PARAMXML = $(WORK_DIR)/parameters.xml .PHONY: firmware firmware: $(PRODUCT_BUNDLE) @@ -497,9 +498,17 @@ $(filter %.S.o,$(OBJS)): $(WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS) $(PRODUCT_BUNDLE): $(PRODUCT_BIN) @$(ECHO) %% Generating $@ +ifdef GEN_PARAM_XML + python $(PX4_BASE)/Tools/px_process_params.py --src-path $(PX4_BASE)/src --xml + $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ + --git_identity $(PX4_BASE) \ + --parameter_xml $(PRODUCT_PARAMXML) \ + --image $< > $@ +else $(Q) $(MKFW) --prototype $(IMAGE_DIR)/$(BOARD).prototype \ --git_identity $(PX4_BASE) \ --image $< > $@ +endif $(PRODUCT_BIN): $(PRODUCT_ELF) $(call SYM_TO_BIN,$<,$@) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 71c7fb49fa..84e5cd08a2 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -50,11 +50,11 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) -ifeq (,$(findstring $(CROSSDEV_VER_SUPPORTED),$(CROSSDEV_VER_FOUND))) -$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of $(CROSSDEV_VER_SUPPORTED).x) +ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) +$(error Unsupported version of $(CC), found: $(CROSSDEV_VER_FOUND) instead of one in: $(CROSSDEV_VER_SUPPORTED)) endif diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 7b2ea703a2..e7318e5196 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index e70320aaa4..f3ce53b4ae 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m4 \ -mthumb \ -march=armv7e-m \ diff --git a/nuttx-configs/px4fmu-v2/scripts/ld.script b/nuttx-configs/px4fmu-v2/scripts/ld.script index 1be39fb87f..bec896d1ce 100644 --- a/nuttx-configs/px4fmu-v2/scripts/ld.script +++ b/nuttx-configs/px4fmu-v2/scripts/ld.script @@ -50,7 +50,8 @@ MEMORY { - flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + /* disabled due to silicon errata flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K */ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 1008K sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K } diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 712631f471..7a0792ff64 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -53,7 +53,7 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index cd2d8eba3b..1717464d2f 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -53,15 +53,11 @@ NM = $(CROSSDEV)nm OBJCOPY = $(CROSSDEV)objcopy OBJDUMP = $(CROSSDEV)objdump -MAXOPTIMIZATION = -O3 +MAXOPTIMIZATION = -Os ARCHCPUFLAGS = -mcpu=cortex-m3 \ -mthumb \ -march=armv7-m -# enable precise stack overflow tracking -#INSTRUMENTATIONDEFINES = -finstrument-functions \ -# -ffixed-r10 - # use our linker script LDSCRIPT = ld.script diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 293690d274..6db6713c4e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -147,7 +147,7 @@ Airspeed::init() _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); if (_airspeed_pub < 0) - warnx("failed to create airspeed sensor object. uORB started?"); + warnx("uORB started?"); } ret = OK; @@ -159,13 +159,15 @@ out: int Airspeed::probe() { - /* on initial power up the device needs more than one retry - for detection. Once it is running then retries aren't - needed + /* on initial power up the device may need more than one retry + for detection. Once it is running the number of retries can + be reduced */ _retries = 4; int ret = measure(); - _retries = 0; + + // drop back to 2 retries once initialised + _retries = 2; return ret; } diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index e5bb772b3c..9d2c1441de 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -89,8 +89,8 @@ static void usage(const char *reason) { if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d ]\n\n"); + warnx("%s\n", reason); + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("ardrone_interface already running\n"); + warnx("already running\n"); /* this is not an error */ exit(0); } @@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tardrone_interface is running\n"); + warnx("running"); } else { - printf("\tardrone_interface not started\n"); + warnx("not started"); } exit(0); } @@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: tcsetattr: %s", uart_name); close(uart); return -1; } @@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) char *device = "/dev/ttyS1"; - /* welcome user */ - printf("[ardrone_interface] Control started, taking over motors\n"); - /* File descriptors */ int gpios; @@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) struct termios uart_config_original; if (motor_test_mode) { - printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); + warnx("setting 10 %% thrust.\n"); } /* Led animation */ @@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - printf("[ardrone_interface] Motors initialized - ready.\n"); - fflush(stdout); - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(device, &uart_config_original); @@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("No UART, exiting."); thread_running = false; exit(ERROR); } @@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) gpios = ar_multiplexing_init(); if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + warnx("write fail"); thread_running = false; exit(ERROR); } @@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); - fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n"); + warnx("motor init fail"); thread_running = false; exit(ERROR); } @@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int termios_state; if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) { - fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n"); + warnx("ERR: tcsetattr"); } - printf("[ardrone_interface] Restored original UART config, exiting..\n"); - /* close uarts */ close(ardrone_write); ar_multiplexing_deinit(gpios); diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index fc017dd589..4fa24275f1 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios) ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0); if (errcounter != 0) { - fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter); + warnx("Failed %d times", -errcounter); fflush(stdout); } return errcounter; diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk index b53fe0a293..0a2d910091 100644 --- a/src/drivers/boards/aerocore/module.mk +++ b/src/drivers/boards/aerocore/module.mk @@ -6,3 +6,5 @@ SRCS = aerocore_init.c \ aerocore_pwm_servo.c \ aerocore_spi.c \ aerocore_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk index 66b7769173..5e1a27d5a1 100644 --- a/src/drivers/boards/px4fmu-v1/module.mk +++ b/src/drivers/boards/px4fmu-v1/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk index 99d37eecac..103232b0ca 100644 --- a/src/drivers/boards/px4fmu-v2/module.mk +++ b/src/drivers/boards/px4fmu-v2/module.mk @@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \ px4fmu_spi.c \ px4fmu_usb.c \ px4fmu2_led.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk index 2601a1c15a..a7a14dd072 100644 --- a/src/drivers/boards/px4io-v1/module.mk +++ b/src/drivers/boards/px4io-v1/module.mk @@ -4,3 +4,5 @@ SRCS = px4io_init.c \ px4io_pwm_servo.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk index 85f94e8be1..3f0e9a0b3a 100644 --- a/src/drivers/boards/px4io-v2/module.mk +++ b/src/drivers/boards/px4io-v2/module.mk @@ -4,3 +4,5 @@ SRCS = px4iov2_init.c \ px4iov2_pwm_servo.c + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 9d684e3948..67aaa0affc 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -130,7 +130,8 @@ public: enum DeviceBusType { DeviceBusType_UNKNOWN = 0, DeviceBusType_I2C = 1, - DeviceBusType_SPI = 2 + DeviceBusType_SPI = 2, + DeviceBusType_UAVCAN = 3, }; /* diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6873f24b60..b10c3e18a9 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -117,6 +117,23 @@ struct pwm_output_values { unsigned channel_count; }; + +/** + * RC config values for a channel + * + * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a + * param_get() dependency + */ +struct pwm_output_rc_config { + uint8_t channel; + uint16_t rc_min; + uint16_t rc_trim; + uint16_t rc_max; + uint16_t rc_dz; + uint16_t rc_assignment; + bool rc_reverse; +}; + /* * ORB tag for PWM outputs. */ @@ -214,7 +231,19 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /** force safety switch on (to enable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) +#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26) + +/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */ +#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27) + +/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28) + +/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */ +#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29) + +/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */ +#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30) /* * diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6e08390436..bccdf11906 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); return -1; } @@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static const speed_t speed = B9600; if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; } @@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } } - /* Print welcome text */ - warnx("FrSky telemetry interface starting..."); - /* Open UART */ struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5fb4b9ff8d..47c907bd33 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg) void GPS::task_main() { - log("starting"); /* open the serial port */ _serial_fd = ::open(_port, O_RDWR); diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f0dc0c6516..9b5c8133b4 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -442,8 +442,6 @@ HIL::task_main() /* make sure servos are off */ // up_pwm_servo_deinit(); - log("stopping"); - /* note - someone else is responsible for restoring the GPIO config */ /* tell the dtor that we are exiting */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 81f7679650..d4dbf37782 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1349,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr; void start(int bus, enum Rotation rotation); void test(int bus); void reset(int bus); -void info(int bus); +int info(int bus); int calibrate(int bus); void usage(); @@ -1595,17 +1595,23 @@ reset(int bus) /** * Print a little info about the driver. */ -void +int info(int bus) { - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext); - if (g_dev == nullptr) - errx(1, "driver not running"); + int ret = 1; - printf("state @ %p\n", g_dev); - g_dev->print_info(); + HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { + warnx("not running on bus %d", bus); + } else { - exit(0); + warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + + g_dev->print_info(); + ret = 0; + } + + return ret; } void @@ -1685,8 +1691,21 @@ hmc5883_main(int argc, char *argv[]) /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) - hmc5883::info(bus); + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + if (bus == -1) { + int ret = 0; + if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + ret = 1; + } + + if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + ret = 1; + } + exit(ret); + } else { + exit(hmc5883::info(bus)); + } + } /* * Autocalibrate the scaling diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index 5daa01dc57..be2ee72766 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -42,3 +42,5 @@ SRCS = hmc5883.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index cb8bbba37e..60a49b559c 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -55,7 +55,7 @@ open_uart(const char *device) const int uart = open(device, O_RDWR | O_NOCTTY); if (uart < 0) { - err(1, "Error opening port: %s", device); + err(1, "ERR: opening %s", device); } /* Back up the original uart configuration to restore it after exit */ @@ -63,7 +63,7 @@ open_uart(const char *device) struct termios uart_config_original; if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + err(1, "ERR: %s: %d", device, termios_state); } /* Fill the struct for the new configuration */ @@ -76,13 +76,13 @@ open_uart(const char *device) /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + err(1, "ERR: %s (tcsetattr)", device); } /* Activate single wire mode */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index a3d3a39333..8ab9d8d55f 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index d293f9954e..edbb14172e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("deamon already running"); + warnx("already running"); exit(0); } @@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("daemon is running"); + warnx("is running"); } else { - warnx("daemon not started"); + warnx("not started"); } exit(0); diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk index 5630e7aeca..3d64d62be0 100644 --- a/src/drivers/l3gd20/module.mk +++ b/src/drivers/l3gd20/module.mk @@ -8,3 +8,5 @@ SRCS = l3gd20.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk index b4f3974f4d..0421eb113e 100644 --- a/src/drivers/lsm303d/module.mk +++ b/src/drivers/lsm303d/module.mk @@ -8,3 +8,5 @@ SRCS = lsm303d.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1d9a463ad3..ba46de3798 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -519,7 +519,7 @@ test() ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + errx(1, "timed out"); } /* now go get it */ diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk index 5b4893b127..da9fcc0fc4 100644 --- a/src/drivers/mpu6000/module.mk +++ b/src/drivers/mpu6000/module.mk @@ -42,3 +42,5 @@ SRCS = mpu6000.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 6c68636c6d..09ec4bf964 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -73,15 +73,13 @@ #include /* Configuration Constants */ -#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION #define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 - + /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x00 /* Measure Register */ - -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ +#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -92,28 +90,42 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -//struct i2c_frame -//{ -// uint16_t frame_count; -// int16_t pixel_flow_x_sum; -// int16_t pixel_flow_y_sum; -// int16_t flow_comp_m_x; -// int16_t flow_comp_m_y; -// int16_t qual; -// int16_t gyro_x_rate; -// int16_t gyro_y_rate; -// int16_t gyro_z_rate; -// uint8_t gyro_range; -// uint8_t sonar_timestamp; -// int16_t ground_distance; -//}; -// -//struct i2c_frame f; +struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; +}; +struct i2c_frame f; -class PX4FLOW : public device::I2C +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; +} __attribute__((packed)); +struct i2c_integral_frame f_integral; + + +class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); virtual int init(); @@ -122,8 +134,8 @@ public: virtual int ioctl(struct file *filp, int cmd, unsigned long arg); /** - * Diagnostics - print some basic information about the driver. - */ + * Diagnostics - print some basic information about the driver. + */ void print_info(); protected: @@ -144,42 +156,41 @@ private: perf_counter_t _buffer_overflows; /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ int probe_address(uint8_t address); /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ void start(); /** - * Stop the automatic measurement state machine. - */ + * Stop the automatic measurement state machine. + */ void stop(); /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ void cycle(); int measure(); int collect(); /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); }; @@ -189,7 +200,7 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz _reports(nullptr), _sensor_ok(false), _measure_ticks(0), @@ -200,7 +211,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls - _debug_enabled = true; + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -228,21 +239,12 @@ PX4FLOW::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); + _reports = new RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { goto out; } - /* get a publish handle on the px4flow topic */ - struct optical_flow_s zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); - - if (_px4flow_topic < 0) { - debug("failed to create px4flow object. Did you start uOrb?"); - } - ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; @@ -410,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) break; } - /* wait for it to complete */ - usleep(PX4FLOW_CONVERSION_INTERVAL); - /* run the collection phase */ if (OK != collect()) { ret = -EIO; @@ -442,8 +441,7 @@ PX4FLOW::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); - printf("i2c::transfer flow returned %d"); + debug("i2c::transfer returned %d", ret); return ret; } @@ -455,52 +453,96 @@ PX4FLOW::measure() int PX4FLOW::collect() { - int ret = -EIO; + int ret = -EIO; /* read from the sensor */ - uint8_t val[22] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t val[47] = { 0 }; perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 22); + if (PX4FLOW_REG == 0x00) { + ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2) + } + + if (PX4FLOW_REG == 0x16) { + ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2) + } if (ret < 0) { - log("error reading from sensor: %d", ret); + debug("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } -// f.frame_count = val[1] << 8 | val[0]; -// f.pixel_flow_x_sum= val[3] << 8 | val[2]; -// f.pixel_flow_y_sum= val[5] << 8 | val[4]; -// f.flow_comp_m_x= val[7] << 8 | val[6]; -// f.flow_comp_m_y= val[9] << 8 | val[8]; -// f.qual= val[11] << 8 | val[10]; -// f.gyro_x_rate= val[13] << 8 | val[12]; -// f.gyro_y_rate= val[15] << 8 | val[14]; -// f.gyro_z_rate= val[17] << 8 | val[16]; -// f.gyro_range= val[18]; -// f.sonar_timestamp= val[19]; -// f.ground_distance= val[21] << 8 | val[20]; + if (PX4FLOW_REG == 0) { + f.frame_count = val[1] << 8 | val[0]; + f.pixel_flow_x_sum = val[3] << 8 | val[2]; + f.pixel_flow_y_sum = val[5] << 8 | val[4]; + f.flow_comp_m_x = val[7] << 8 | val[6]; + f.flow_comp_m_y = val[9] << 8 | val[8]; + f.qual = val[11] << 8 | val[10]; + f.gyro_x_rate = val[13] << 8 | val[12]; + f.gyro_y_rate = val[15] << 8 | val[14]; + f.gyro_z_rate = val[17] << 8 | val[16]; + f.gyro_range = val[18]; + f.sonar_timestamp = val[19]; + f.ground_distance = val[21] << 8 | val[20]; + + f_integral.frame_count_since_last_readout = val[23] << 8 | val[22]; + f_integral.pixel_flow_x_integral = val[25] << 8 | val[24]; + f_integral.pixel_flow_y_integral = val[27] << 8 | val[26]; + f_integral.gyro_x_rate_integral = val[29] << 8 | val[28]; + f_integral.gyro_y_rate_integral = val[31] << 8 | val[30]; + f_integral.gyro_z_rate_integral = val[33] << 8 | val[32]; + f_integral.integration_timespan = val[37] << 24 | val[36] << 16 + | val[35] << 8 | val[34]; + f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16 + | val[39] << 8 | val[38]; + f_integral.ground_distance = val[43] << 8 | val[42]; + f_integral.gyro_temperature = val[45] << 8 | val[44]; + f_integral.qual = val[46]; + } + + if (PX4FLOW_REG == 0x16) { + f_integral.frame_count_since_last_readout = val[1] << 8 | val[0]; + f_integral.pixel_flow_x_integral = val[3] << 8 | val[2]; + f_integral.pixel_flow_y_integral = val[5] << 8 | val[4]; + f_integral.gyro_x_rate_integral = val[7] << 8 | val[6]; + f_integral.gyro_y_rate_integral = val[9] << 8 | val[8]; + f_integral.gyro_z_rate_integral = val[11] << 8 | val[10]; + f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12]; + f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16]; + f_integral.ground_distance = val[21] << 8 | val[20]; + f_integral.gyro_temperature = val[23] << 8 | val[22]; + f_integral.qual = val[24]; + } - int16_t flowcx = val[7] << 8 | val[6]; - int16_t flowcy = val[9] << 8 | val[8]; - int16_t gdist = val[21] << 8 | val[20]; struct optical_flow_s report; - report.flow_comp_x_m = float(flowcx) / 1000.0f; - report.flow_comp_y_m = float(flowcy) / 1000.0f; - report.flow_raw_x = val[3] << 8 | val[2]; - report.flow_raw_y = val[5] << 8 | val[4]; - report.ground_distance_m = float(gdist) / 1000.0f; - report.quality = val[10]; - report.sensor_id = 0; + report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual; //0:bad ; 255 max quality + report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians + report.integration_timespan = f_integral.integration_timespan; //microseconds + report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius + report.sensor_id = 0; - /* publish it */ - orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + if (_px4flow_topic < 0) { + _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); + + } else { + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + } /* post a report to the ring */ if (_reports->force(&report)) { @@ -560,50 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg) void PX4FLOW::cycle() { - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - if (OK != collect()) { - log("collection error"); - /* restart the measurement state machine */ - start(); - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ if (OK != measure()) { - log("measure error"); + debug("measure error"); } - /* next phase is collection */ - _collect_phase = true; + /* perform collection */ + if (OK != collect()) { + debug("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, + _measure_ticks); - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&PX4FLOW::cycle_trampoline, - this, - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); } void @@ -649,14 +662,41 @@ start() } /* create the driver */ - g_dev = new PX4FLOW(PX4FLOW_BUS); + g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); if (g_dev == nullptr) { goto fail; } if (OK != g_dev->init()) { - goto fail; + + #ifdef PX4_I2C_BUS_ESC + delete g_dev; + /* try 2nd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + #endif + + delete g_dev; + /* try 3rd bus */ + g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + #ifdef PX4_I2C_BUS_ESC + } + #endif } /* set the poll rate to default, starts automatic data collection */ @@ -685,7 +725,8 @@ fail: /** * Stop the driver */ -void stop() +void +stop() { if (g_dev != nullptr) { delete g_dev; @@ -716,6 +757,7 @@ test() err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH); } + /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -725,18 +767,18 @@ test() } warnx("single read"); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); - warnx("time: %lld", report.timestamp); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); - - /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - errx(1, "failed to set 2Hz poll rate"); + /* start the sensor polling at 10Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) { + errx(1, "failed to set 10Hz poll rate"); } /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { + for (unsigned i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -756,9 +798,22 @@ test() } warnx("periodic read %u", i); - warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m); - warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m); - warnx("time: %lld", report.timestamp); + + warnx("framecount_total: %u", f.frame_count); + warnx("framecount_integral: %u", + f_integral.frame_count_since_last_readout); + warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral); + warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral); + warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral); + warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral); + warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral); + warnx("integration_timespan [us]: %u", f_integral.integration_timespan); + warnx("ground_distance: %0.2f m", + (double) f_integral.ground_distance / 1000); + warnx("time since last sonar update [us]: %i", + f_integral.time_since_last_sonar_update); + warnx("quality integration average : %i", f_integral.qual); + warnx("quality : %i", f.qual); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index a60f1a434a..a06323a52f 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -8,3 +8,5 @@ SRCS = fmu.cpp MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 5b838fb75e..924283356b 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d68d9f604..58390ba4c3 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -817,6 +817,11 @@ PX4IO::init() } + /* set safety to off if circuit breaker enabled */ + if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) { + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); + } + /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this); @@ -1155,52 +1160,54 @@ PX4IO::io_set_arming_state() actuator_armed_s armed; ///< system armed state vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); + int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed) { - set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + if (have_armed == OK) { + if (armed.armed) { + set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } else { + clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + } - } else { - clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; + if (armed.lockdown && !_lockdown_override) { + set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } else { + clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; + } + + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !_cb_flighttermination) { + set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } else { + clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + + if (armed.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } } - if (armed.lockdown && !_lockdown_override) { - set |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } else { - clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; - } - - /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !_cb_flighttermination) { - set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } else { - clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; - } - - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } - - 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; + if (have_control_mode == 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; + } } return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); @@ -1245,41 +1252,42 @@ PX4IO::io_set_rc_config() * for compatibility reasons with existing * autopilots / GCS'. */ + + /* ROLL */ param_get(param_find("RC_MAP_ROLL"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 0; + } - /* subtract one from 1-based index - this might be - * a negative number now - */ - ichan -= 1; - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; - + /* PITCH */ param_get(param_find("RC_MAP_PITCH"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 1; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; - + /* YAW */ param_get(param_find("RC_MAP_YAW"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 2; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; - + /* THROTTLE */ param_get(param_find("RC_MAP_THROTTLE"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 3; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; - + /* FLAPS */ param_get(param_find("RC_MAP_FLAPS"), &ichan); + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { + input_map[ichan - 1] = 4; + } - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 4; - + /* MAIN MODE SWITCH */ param_get(param_find("RC_MAP_MODE_SW"), &ichan); - - if ((ichan >= 0) && (ichan < (int)_max_rc_input)) { + if ((ichan > 0) && (ichan <= (int)_max_rc_input)) { /* use out of normal bounds index to indicate special channel */ - input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; + input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH; } /* @@ -1656,10 +1664,6 @@ PX4IO::io_publish_raw_rc() int PX4IO::io_publish_pwm_outputs() { - /* if no FMU comms(!) just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK)) - return OK; - /* data we are going to fetch */ actuator_outputs_s outputs; outputs.timestamp = hrt_absolute_time(); @@ -2060,7 +2064,7 @@ PX4IO::print_status(bool extended_status) ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%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"), @@ -2070,7 +2074,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), - ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2195,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count); @@ -2214,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count); @@ -2233,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count); @@ -2252,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) struct pwm_output_values* pwm = (struct pwm_output_values*)arg; if (pwm->channel_count > _max_actuators) /* fail with error */ - return E2BIG; + return -E2BIG; /* copy values to registers in IO */ ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count); @@ -2310,6 +2315,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_OVERRIDE_IMMEDIATE: + /* control whether override on FMU failure is + immediate or waits for override threshold on mode + switch */ + if (arg == 0) { + /* clear override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0); + } else { + /* set override immediate flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ @@ -2571,6 +2589,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_RC_CONFIG: { + /* enable setting of RC configuration without relying + on param_get() + */ + struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg; + if (config->channel >= RC_INPUT_MAX_CHANNELS) { + /* fail with error */ + return -E2BIG; + } + + /* copy values to registers in IO */ + uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE]; + uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE; + regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min; + regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim; + regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max; + regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz; + regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment; + regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (config->rc_reverse) { + regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE; + } + ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); + break; + } + + case PWM_SERVO_SET_OVERRIDE_OK: + /* set the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK); + break; + + case PWM_SERVO_CLEAR_OVERRIDE_OK: + /* clear the 'OVERRIDE OK' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + break; + default: /* see if the parent class can make any use of it */ ret = CDev::ioctl(filep, cmd, arg); diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 13cbfdfa8b..d357222441 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -121,7 +121,7 @@ private: /* for now, we only support one RGBLED */ namespace { -RGBLED *g_rgbled; +RGBLED *g_rgbled = nullptr; } void rgbled_usage(); @@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[]) ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ + if (!strcmp(verb, "stop")) { + delete g_rgbled; + g_rgbled = nullptr; + exit(0); + } exit(ret); } - if (!strcmp(verb, "stop")) { - delete g_rgbled; - g_rgbled = nullptr; - exit(0); - } - if (!strcmp(verb, "rgb")) { if (argc < 5) { errx(1, "Usage: rgbled rgb "); diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk index 48620feea8..38ea490a0e 100644 --- a/src/drivers/stm32/adc/module.mk +++ b/src/drivers/stm32/adc/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = adc SRCS = adc.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk index bb751c7f68..54428e270d 100644 --- a/src/drivers/stm32/module.mk +++ b/src/drivers/stm32/module.mk @@ -41,3 +41,5 @@ SRCS = drv_hrt.c \ drv_pwm_servo.c INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk index 827cf30b27..25a194ef61 100644 --- a/src/drivers/stm32/tone_alarm/module.mk +++ b/src/drivers/stm32/tone_alarm/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm SRCS = tone_alarm.cpp INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/trone/module.mk b/src/drivers/trone/module.mk new file mode 100644 index 0000000000..38499c6c3e --- /dev/null +++ b/src/drivers/trone/module.mk @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Makefile to build the TeraRanger One range finder driver +# + +MODULE_COMMAND = trone + +SRCS = trone.cpp + +MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp new file mode 100644 index 0000000000..2f2f692a10 --- /dev/null +++ b/src/drivers/trone/trone.cpp @@ -0,0 +1,913 @@ +/**************************************************************************** + * + * 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 trone.cpp + * @author Luis Rodrigues + * + * Driver for the TeraRanger One range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define TRONE_BUS PX4_I2C_BUS_EXPANSION +#define TRONE_BASEADDR 0x30 /* 7-bit address */ +#define TRONE_DEVICE_PATH "/dev/trone" + +/* TRONE Registers addresses */ + +#define TRONE_MEASURE_REG 0x00 /* Measure range register */ + +/* Device limits */ +#define TRONE_MIN_DISTANCE (0.20f) +#define TRONE_MAX_DISTANCE (14.00f) + +#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class TRONE : public device::I2C +{ +public: + TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR); + virtual ~TRONE(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE + * and TRONE_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +static const uint8_t crc_table[] = { + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, + 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, + 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, + 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, + 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, + 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, + 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, + 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, + 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, + 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, + 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, + 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, + 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, + 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, + 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, + 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, + 0xfa, 0xfd, 0xf4, 0xf3 +}; + +uint8_t crc8(uint8_t *p, uint8_t len){ + uint16_t i; + uint16_t crc = 0x0; + + while (len--) { + i = (crc ^ *p++) & 0xFF; + crc = (crc_table[i] ^ (crc << 8)) & 0xFF; + } + + return crc & 0xFF; +} + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int trone_main(int argc, char *argv[]); + +TRONE::TRONE(int bus, int address) : + I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000), + _min_distance(TRONE_MIN_DISTANCE), + _max_distance(TRONE_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "trone_read")), + _comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows")) +{ + // up the retries since the device misses the first measure attempts + I2C::_retries = 3; + + // enable debug() calls + _debug_enabled = false; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +TRONE::~TRONE() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +TRONE::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report; + measure(); + _reports->get(&rf_report); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +TRONE::probe() +{ + return measure(); +} + +void +TRONE::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +TRONE::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +TRONE::get_minimum_distance() +{ + return _min_distance; +} + +float +TRONE::get_maximum_distance() +{ + return _max_distance; +} + +int +TRONE::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* 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 _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +TRONE::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* 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 (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(TRONE_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +TRONE::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + const uint8_t cmd = TRONE_MEASURE_REG; + ret = transfer(&cmd, sizeof(cmd), nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +TRONE::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[3] = {0, 0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 3); + + 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 * 0.001f; /* mm 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 */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + report.distance = si_units; + report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +TRONE::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +TRONE::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +TRONE::cycle_trampoline(void *arg) +{ + TRONE *dev = (TRONE *)arg; + + dev->cycle(); +} + +void +TRONE::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + log("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); +} + +void +TRONE::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace trone +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +TRONE *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new TRONE(TRONE_BUS); + + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 50x and report each value */ + for (unsigned i = 0; i < 50; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("valid %u", report.valid); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(TRONE_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +trone_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + trone::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + trone::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + trone::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + trone::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + trone::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} 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 c775428ef1..0b8c01f798 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (vehicle_liftoff || params.debug) { /* copy flow */ - flow_speed[0] = flow.flow_comp_x_m; - flow_speed[1] = flow.flow_comp_y_m; + flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; + flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m; flow_speed[2] = 0.0f; /* convert to bodyframe velocity */ diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0ed210cf4c..cfcc48b62a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; } - // Calculate PD + FF throttle + // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; + _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf); // 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 - thrRateIncr, + _last_throttle_dem + thrRateIncr); } // Ensure _last_throttle_dem is always initialized properly - // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! _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 + // Set to a value that 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 + // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_climbOutDem) { diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 0000000000..fea1a773e3 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -0,0 +1,298 @@ +function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + + +%LQG Postion Estimator and Controller +% Observer: +% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) +% x[n+1|n] = Ax[n|n] + Bu[n] +% +% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ +% +% +% Arguments: +% approx_prediction: if 1 then the exponential map is approximated with a +% first order taylor approximation. has at the moment not a big influence +% (just 1st or 2nd order approximation) we should change it to rodriquez +% approximation. +% use_inertia_matrix: set to true if you have the inertia matrix J for your +% quadrotor +% xa_apo_k: old state vectotr +% zFlag: if sensor measurement is available [gyro, acc, mag] +% dt: dt in s +% z: measurements [gyro, acc, mag] +% q_rotSpeed: process noise gyro +% q_rotAcc: process noise gyro acceleration +% q_acc: process noise acceleration +% q_mag: process noise magnetometer +% r_gyro: measurement noise gyro +% r_accel: measurement noise accel +% r_mag: measurement noise mag +% J: moment of inertia matrix + + +% Output: +% xa_apo: updated state vectotr +% Pa_apo: updated state covariance matrix +% Rot_matrix: rotation matrix +% eulerAngles: euler angles +% debugOutput: not used + + +%% model specific parameters + + + +% compute once the inverse of the Inertia +persistent Ji; +if isempty(Ji) + Ji=single(inv(J)); +end + +%% init +persistent x_apo +if(isempty(x_apo)) + gyro_init=single([0;0;0]); + gyro_acc_init=single([0;0;0]); + acc_init=single([0;0;-9.81]); + mag_init=single([1;0;0]); + x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); + +end + +persistent P_apo +if(isempty(P_apo)) + % P_apo = single(eye(NSTATES) * 1000); + P_apo = single(200*ones(12)); +end + +debugOutput = single(zeros(4,1)); + +%% copy the states +wx= x_apo(1); % x body angular rate +wy= x_apo(2); % y body angular rate +wz= x_apo(3); % z body angular rate + +wax= x_apo(4); % x body angular acceleration +way= x_apo(5); % y body angular acceleration +waz= x_apo(6); % z body angular acceleration + +zex= x_apo(7); % x component gravity vector +zey= x_apo(8); % y component gravity vector +zez= x_apo(9); % z component gravity vector + +mux= x_apo(10); % x component magnetic field vector +muy= x_apo(11); % y component magnetic field vector +muz= x_apo(12); % z component magnetic field vector + + + + +%% prediction section +% compute the apriori state estimate from the previous aposteriori estimate +%body angular accelerations +if (use_inertia_matrix==1) + wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; +else + wak =[wax;way;waz]; +end + +%body angular rates +wk =[wx; wy; wz] + dt*wak; + +%derivative of the prediction rotation matrix +O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; + +%prediction of the earth z vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + zek =(O*dt+single(eye(3)))*[zex;zey;zez]; + +else + zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; + %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double + %precision +end + + + +%prediction of the magnetic vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + muk =(O*dt+single(eye(3)))*[mux;muy;muz]; +else + muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; + %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double + %precision +end + +x_apr=[wk;wak;zek;muk]; + +% compute the apriori error covariance estimate from the previous +%aposteriori estimate + +EZ=[0,zez,-zey; + -zez,0,zex; + zey,-zex,0]'; +MA=[0,muz,-muy; + -muz,0,mux; + muy,-mux,0]'; + +E=single(eye(3)); +Z=single(zeros(3)); + +A_lin=[ Z, E, Z, Z + Z, Z, Z, Z + EZ, Z, O, Z + MA, Z, Z, O]; + +A_lin=eye(12)+A_lin*dt; + +%process covariance matrix + +persistent Q +if (isempty(Q)) + Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... + q_rotAcc,q_rotAcc,q_rotAcc,... + q_acc,q_acc,q_acc,... + q_mag,q_mag,q_mag]); +end + +P_apr=A_lin*P_apo*A_lin'+Q; + + +%% update +if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 + +% R=[r_gyro,0,0,0,0,0,0,0,0; +% 0,r_gyro,0,0,0,0,0,0,0; +% 0,0,r_gyro,0,0,0,0,0,0; +% 0,0,0,r_accel,0,0,0,0,0; +% 0,0,0,0,r_accel,0,0,0,0; +% 0,0,0,0,0,r_accel,0,0,0; +% 0,0,0,0,0,0,r_mag,0,0; +% 0,0,0,0,0,0,0,r_mag,0; +% 0,0,0,0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; + %observation matrix + %[zw;ze;zmk]; + H_k=[ E, Z, Z, Z; + Z, Z, E, Z; + Z, Z, Z, E]; + + y_k=z(1:9)-H_k*x_apr; + + + %S_k=H_k*P_apr*H_k'+R; + S_k=H_k*P_apr*H_k'; + S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; + K_k=(P_apr*H_k'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k)*P_apr; +else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 + + R=[r_gyro,0,0; + 0,r_gyro,0; + 0,0,r_gyro]; + R_v=[r_gyro,r_gyro,r_gyro]; + %observation matrix + + H_k=[ E, Z, Z, Z]; + + y_k=z(1:3)-H_k(1:3,1:12)*x_apr; + + % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; + S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; + K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 + +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_accel,0,0; +% 0,0,0,0,r_accel,0; +% 0,0,0,0,0,r_accel]; + + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, E, Z]; + + y_k=z(1:6)-H_k(1:6,1:12)*x_apr; + + % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_mag,0,0; +% 0,0,0,0,r_mag,0; +% 0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, Z, E]; + + y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; + + %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + x_apo=x_apr; + P_apo=P_apr; + end + end + end +end + + + +%% euler anglels extraction +z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); +m_n_b = x_apo(10:12)./norm(x_apo(10:12)); + +y_n_b=cross(z_n_b,m_n_b); +y_n_b=y_n_b./norm(y_n_b); + +x_n_b=(cross(y_n_b,z_n_b)); +x_n_b=x_n_b./norm(x_n_b); + + +xa_apo=x_apo; +Pa_apo=P_apo; +% rotation matrix from earth to body system +Rot_matrix=[x_n_b,y_n_b,z_n_b]; + + +phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); +theta=-asin(Rot_matrix(1,3)); +psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); +eulerAngles=[phi;theta;psi]; + diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100644 index 0000000000..9ea5203463 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,502 @@ + + + + + false + false + false + option.WorkingFolder.Project + + option.BuildFolder.Project + + + true + true + true + true + option.GlobalDataSyncMethod.SyncAlways + true + option.DynamicMemoryAllocation.Threshold + 65536 + 200000 + option.FilePartitionMethod.MapMFileToCFile + true + false + + true + false + true + false + + + + + + + + + true + false + 40000 + 100 + option.TargetLang.C + true + 10 + 200 + 4000 + true + 64 + true + true + option.ConstantInputs.CheckValues + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + option.WorkingFolder.Project + + option.BuildFolder.Specified + codegen + + true + false + false + false + option.DynamicMemoryAllocation.Threshold + 65536 + 4000 + false + option.FilePartitionMethod.SingleFile + true + true + false + option.DataTypeReplacement.CBuiltIn + false + true + option.ParenthesesLevel.Nominal + 31 + $M$N + $M$N + $M$N + $M$N + $M$N + $M$N + emxArray_$M$N + emx$M$N + + true + false + true + true + false + true + + + + + + + + + C89/C90 (ANSI) + true + ARM Compatible + ARM Cortex + true + 8 + 16 + 32 + 32 + 64 + 32 + 64 + 32 + 32 + option.HardwareEndianness.Little + true + false + option.HardwareAtomicIntegerSize.Long + option.HardwareAtomicFloatSize.Double + option.HardwareDivisionRounding.Undefined + Generic + MATLAB Host Computer + false + 8 + 16 + 32 + 64 + 64 + 32 + 64 + 64 + 64 + option.HardwareEndianness.Little + true + true + option.HardwareAtomicIntegerSize.Char + option.HardwareAtomicFloatSize.None + option.HardwareDivisionRounding.Zero + Automatically locate an installed toolchain + Faster Builds + + 40000 + 100 + true + option.TargetLang.C + option.CCompilerOptimization.On + + true + false + make_rtw + default_tmf + + 10 + 200 + 4000 + true + 64 + true + true + false + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + R2012a + true + t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html + /home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html + true + option.VerificationMode.None + false + ${PROJECT_ROOT}/AttitudeEKF_Test.m + ${PROJECT_ROOT}/AttitudeEKF_Test.m + false + false + 1024 + 2048 + AttitudeEKF_mex + AttitudeEKF + option.target.artifact.lib + option.FixedPointTypeProposalMode.ProposeFractionLengths + numerictype([],16,12) + 0 + true + false + false + false + true + false + + option.FixedPointMode.None + false + + + 16 + 4 + 0 + fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128) + option.FixedPointTypeSource.SimAndDerived + + false + false + false + true + + + + + + + + + true + _fixpt + option.DefaultFixedPointSignedness.Automatic + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 1 x 1 + false + + + uint8 + false + 3 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 9 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 1 x 1 + false + + + single + false + 3 x 3 + false + + + + + + ${PROJECT_ROOT}/AttitudeEKF_Test.m + + + + /opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a + + + + /opt/matlab/r2013b + + + + + + true + false + false + false + false + false + true + false + 3.16.1-1-ARCH + false + true + glnxa64 + true + + + + diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6a..c1f032b49b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -74,8 +75,7 @@ #ifdef __cplusplus extern "C" { #endif -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" +#include "codegen/AttitudeEKF.h" #include "attitude_estimator_ekf_params.h" #ifdef __cplusplus } @@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 14000, + 7200, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); @@ -206,14 +206,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); + float debugOutput[4] = { 0.0f }; int overloadcounter = 19; /* Initialize filter */ - attitudeKalmanfilter_initialize(); + AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); @@ -277,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params; + struct attitude_estimator_ekf_params ekf_params = { 0 }; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -508,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* Call the estimator */ + AttitudeEKF(false, // approx_prediction + (unsigned char)ekf_params.use_moment_inertia, + update_vect, + dt, + z_k, + ekf_params.q[0], // q_rotSpeed, + ekf_params.q[1], // q_rotAcc + ekf_params.q[2], // q_acc + ekf_params.q[3], // q_mag + ekf_params.r[0], // r_gyro + ekf_params.r[1], // r_accel + ekf_params.r[2], // r_mag + ekf_params.moment_inertia_J, + x_aposteriori, + P_aposteriori, + Rot_matrix, + euler, + debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index bc0e3b93a2..5637ec102f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,28 +44,96 @@ /* Extended Kalman Filter covariances */ -/* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); -/* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); -/* gyro measurement noise */ +/** + * Body angular rate process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); + +/** + * Body angular acceleration process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); + +/** + * Acceleration process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); + +/** + * Gyro measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); -/* accel measurement noise */ + +/** + * Accel measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); -/* mag measurement noise */ + +/** + * Mag measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); -/* offset estimation - UNUSED */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); +/** + * Moment of inertia matrix diagonal entry (1, 1) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (2, 2) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (3, 3) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); + +/** + * Moment of inertia enabled in estimator + * + * If set to != 0 the moment of inertia will be used in the estimator + * + * @group attitude_ekf + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_J_EN, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q1 = param_find("EKF_ATT_V3_Q1"); h->q2 = param_find("EKF_ATT_V3_Q2"); h->q3 = param_find("EKF_ATT_V3_Q3"); - h->q4 = param_find("EKF_ATT_V3_Q4"); h->r0 = param_find("EKF_ATT_V4_R0"); h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->r3 = param_find("EKF_ATT_V4_R3"); h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); + h->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + h->use_moment_inertia = param_find("ATT_J_EN"); + return OK; } @@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q1, &(p->q[1])); param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); - param_get(h->q4, &(p->q[4])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); + for (int i = 0; i < 3; i++) { + param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); + } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 5985541cac..5d3b6b2440 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -42,8 +42,10 @@ #include struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; + float r[3]; + float q[4]; + float moment_inertia_J[9]; + int32_t use_moment_inertia; float roll_off; float pitch_off; float yaw_off; @@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3; - param_t q0, q1, q2, q3, q4; + param_t r0, r1, r2; + param_t q0, q1, q2, q3; + param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ + param_t use_moment_inertia; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c new file mode 100644 index 0000000000..68db382cfe --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -0,0 +1,1456 @@ +/* + * AttitudeEKF.c + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +/* Include files */ +#include "AttitudeEKF.h" + +/* Variable Definitions */ +static float Ji[9]; +static boolean_T Ji_not_empty; +static float x_apo[12]; +static float P_apo[144]; +static float Q[144]; +static boolean_T Q_not_empty; + +/* Function Declarations */ +static void AttitudeEKF_init(void); +static void b_mrdivide(const float A[72], const float B[36], float y[72]); +static void inv(const float x[9], float y[9]); +static void mrdivide(const float A[108], const float B[81], float y[108]); +static float norm(const float x[3]); + +/* Function Definitions */ +static void AttitudeEKF_init(void) +{ + int i; + static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + -9.81F, 1.0F, 0.0F, 0.0F }; + + for (i = 0; i < 12; i++) { + x_apo[i] = fv5[i]; + } + + for (i = 0; i < 144; i++) { + P_apo[i] = 200.0F; + } +} + +/* + * + */ +static void b_mrdivide(const float A[72], const float B[36], float y[72]) +{ + float b_A[36]; + signed char ipiv[6]; + int i1; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[72]; + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i1] = B[i1 + 6 * iy]; + } + + ipiv[i1] = (signed char)(1 + i1); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; + } + } + + i1 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i1; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i1 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i1 = 0; i1 < 12; i1++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i1] = A[i1 + 12 * iy]; + } + } + + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i1] = Y[i1 + 6 * iy]; + } + } +} + +/* + * + */ +static void inv(const float x[9], float y[9]) +{ + float b_x[9]; + int p1; + int p2; + int p3; + float absx11; + float absx21; + float absx31; + int itmp; + for (p1 = 0; p1 < 9; p1++) { + b_x[p1] = x[p1]; + } + + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = (real32_T)fabs(x[0]); + absx21 = (real32_T)fabs(x[1]); + absx31 = (real32_T)fabs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + absx11 = b_x[1] / b_x[0]; + b_x[1] /= b_x[0]; + absx21 = b_x[2] / b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= absx11 * b_x[3]; + b_x[5] -= absx21 * b_x[3]; + b_x[7] -= absx11 * b_x[6]; + b_x[8] -= absx21 * b_x[6]; + if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + b_x[1] = absx21; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + absx11 = b_x[5] / b_x[4]; + b_x[5] /= b_x[4]; + b_x[8] -= absx11 * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0F / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; +} + +/* + * + */ +static void mrdivide(const float A[108], const float B[81], float y[108]) +{ + float b_A[81]; + signed char ipiv[9]; + int i0; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[108]; + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i0] = B[i0 + 9 * iy]; + } + + ipiv[i0] = (signed char)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i0 = 0; i0 < 12; i0++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i0] = A[i0 + 12 * iy]; + } + } + + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i0] = Y[i0 + 9 * iy]; + } + } +} + +/* + * + */ +static float norm(const float x[3]) +{ + float y; + float scale; + int k; + float absxk; + float t; + y = 0.0F; + scale = 1.17549435E-38F; + for (k = 0; k < 3; k++) { + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + return scale * (real32_T)sqrt(y); +} + +/* + * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + * = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + */ +void AttitudeEKF(unsigned char approx_prediction, unsigned char + use_inertia_matrix, const unsigned char zFlag[3], float dt, + const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, + float q_mag, float r_gyro, float r_accel, float r_mag, const + float J[9], float xa_apo[12], float Pa_apo[144], float + Rot_matrix[9], float eulerAngles[3], float debugOutput[4]) +{ + int i; + float fv0[3]; + int r2; + float zek[3]; + float muk[3]; + float b_muk[3]; + float c_muk[3]; + float fv1[3]; + float wak[3]; + float O[9]; + float b_O[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float fv2[3]; + float maxval; + int r1; + float fv3[9]; + float fv4[3]; + float x_apr[12]; + signed char I[144]; + static float A_lin[144]; + static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static float b_A_lin[144]; + float v[12]; + static float P_apr[144]; + float a[108]; + static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float S_k[81]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[9]; + float K_k[108]; + float b_S_k[36]; + static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 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 }; + + static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float c_r_gyro[3]; + float B[36]; + int r3; + float a21; + float Y[36]; + float d_a[72]; + static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 }; + + float d_r_gyro[6]; + float c_S_k[6]; + float b_K_k[72]; + static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; + + static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1 }; + + float b_z[6]; + + /* LQG Postion Estimator and Controller */ + /* Observer: */ + /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ + /* x[n+1|n] = Ax[n|n] + Bu[n] */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ */ + /* */ + /* */ + /* Arguments: */ + /* approx_prediction: if 1 then the exponential map is approximated with a */ + /* first order taylor approximation. has at the moment not a big influence */ + /* (just 1st or 2nd order approximation) we should change it to rodriquez */ + /* approximation. */ + /* use_inertia_matrix: set to true if you have the inertia matrix J for your */ + /* quadrotor */ + /* xa_apo_k: old state vectotr */ + /* zFlag: if sensor measurement is available [gyro, acc, mag] */ + /* dt: dt in s */ + /* z: measurements [gyro, acc, mag] */ + /* q_rotSpeed: process noise gyro */ + /* q_rotAcc: process noise gyro acceleration */ + /* q_acc: process noise acceleration */ + /* q_mag: process noise magnetometer */ + /* r_gyro: measurement noise gyro */ + /* r_accel: measurement noise accel */ + /* r_mag: measurement noise mag */ + /* J: moment of inertia matrix */ + /* Output: */ + /* xa_apo: updated state vectotr */ + /* Pa_apo: updated state covariance matrix */ + /* Rot_matrix: rotation matrix */ + /* eulerAngles: euler angles */ + /* debugOutput: not used */ + /* % model specific parameters */ + /* compute once the inverse of the Inertia */ + /* 'AttitudeEKF:48' if isempty(Ji) */ + if (!Ji_not_empty) { + /* 'AttitudeEKF:49' Ji=single(inv(J)); */ + inv(J, Ji); + Ji_not_empty = TRUE; + } + + /* % init */ + /* 'AttitudeEKF:54' if(isempty(x_apo)) */ + /* 'AttitudeEKF:64' if(isempty(P_apo)) */ + /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ + for (i = 0; i < 4; i++) { + debugOutput[i] = 0.0F; + } + + /* % copy the states */ + /* 'AttitudeEKF:72' wx= x_apo(1); */ + /* x body angular rate */ + /* 'AttitudeEKF:73' wy= x_apo(2); */ + /* y body angular rate */ + /* 'AttitudeEKF:74' wz= x_apo(3); */ + /* z body angular rate */ + /* 'AttitudeEKF:76' wax= x_apo(4); */ + /* x body angular acceleration */ + /* 'AttitudeEKF:77' way= x_apo(5); */ + /* y body angular acceleration */ + /* 'AttitudeEKF:78' waz= x_apo(6); */ + /* z body angular acceleration */ + /* 'AttitudeEKF:80' zex= x_apo(7); */ + /* x component gravity vector */ + /* 'AttitudeEKF:81' zey= x_apo(8); */ + /* y component gravity vector */ + /* 'AttitudeEKF:82' zez= x_apo(9); */ + /* z component gravity vector */ + /* 'AttitudeEKF:84' mux= x_apo(10); */ + /* x component magnetic field vector */ + /* 'AttitudeEKF:85' muy= x_apo(11); */ + /* y component magnetic field vector */ + /* 'AttitudeEKF:86' muz= x_apo(12); */ + /* z component magnetic field vector */ + /* % prediction section */ + /* compute the apriori state estimate from the previous aposteriori estimate */ + /* body angular accelerations */ + /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ + if (use_inertia_matrix == 1) { + /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ + fv0[0] = x_apo[3]; + fv0[1] = x_apo[4]; + fv0[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += J[r2 + 3 * i] * fv0[i]; + } + } + + muk[0] = x_apo[3]; + muk[1] = x_apo[4]; + muk[2] = x_apo[5]; + b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; + b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; + b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; + for (r2 = 0; r2 < 3; r2++) { + c_muk[r2] = -b_muk[r2]; + } + + fv1[0] = x_apo[3]; + fv1[1] = x_apo[4]; + fv1[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + fv0[r2] = 0.0F; + for (i = 0; i < 3; i++) { + fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; + } + + wak[r2] = fv1[r2] + fv0[r2] * dt; + } + } else { + /* 'AttitudeEKF:96' else */ + /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ + wak[0] = x_apo[3]; + wak[1] = x_apo[4]; + wak[2] = x_apo[5]; + } + + /* body angular rates */ + /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_apo[2]; + O[2] = x_apo[1]; + O[3] = x_apo[2]; + O[4] = 0.0F; + O[5] = -x_apo[0]; + O[6] = -x_apo[1]; + O[7] = x_apo[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'AttitudeEKF:107' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += b_O[r2 + 3 * i] * fv2[i]; + } + } + } else { + /* 'AttitudeEKF:112' else */ + /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += fv3[r2 + 3 * i] * fv2[i]; + } + } + + /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ + /* precision */ + } + + /* prediction of the magnetic vector */ + /* 'AttitudeEKF:121' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += b_O[r2 + 3 * i] * fv4[i]; + } + } + } else { + /* 'AttitudeEKF:125' else */ + /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += fv3[r2 + 3 * i] * fv4[i]; + } + } + + /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ + /* precision */ + } + + /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ + x_apr[0] = x_apo[0] + dt * wak[0]; + x_apr[1] = x_apo[1] + dt * wak[1]; + x_apr[2] = x_apo[2] + dt * wak[2]; + for (i = 0; i < 3; i++) { + x_apr[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 6] = zek[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 9] = muk[i]; + } + + /* compute the apriori error covariance estimate from the previous */ + /* aposteriori estimate */ + /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ + /* 'AttitudeEKF:137' -zez,0,zex; */ + /* 'AttitudeEKF:138' zey,-zex,0]'; */ + /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ + /* 'AttitudeEKF:140' -muz,0,mux; */ + /* 'AttitudeEKF:141' muy,-mux,0]'; */ + /* 'AttitudeEKF:143' E=single(eye(3)); */ + /* 'AttitudeEKF:144' Z=single(zeros(3)); */ + /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ + /* 'AttitudeEKF:147' Z, Z, Z, Z */ + /* 'AttitudeEKF:148' EZ, Z, O, Z */ + /* 'AttitudeEKF:149' MA, Z, Z, O]; */ + /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + for (r2 = 0; r2 < 3; r2++) { + A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; + } + + for (r2 = 0; r2 < 3; r2++) { + A_lin[(r2 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_apo[8]; + A_lin[8] = -x_apo[7]; + A_lin[18] = -x_apo[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_apo[6]; + A_lin[30] = x_apo[7]; + A_lin[31] = -x_apo[6]; + A_lin[32] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_apo[11]; + A_lin[11] = -x_apo[10]; + A_lin[21] = -x_apo[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_apo[9]; + A_lin[33] = x_apo[10]; + A_lin[34] = -x_apo[9]; + A_lin[35] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; + } + } + + /* process covariance matrix */ + /* 'AttitudeEKF:156' if (isempty(Q)) */ + if (!Q_not_empty) { + /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ + /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ + /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ + /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ + v[0] = q_rotSpeed; + v[1] = q_rotSpeed; + v[2] = q_rotSpeed; + v[3] = q_rotAcc; + v[4] = q_rotAcc; + v[5] = q_rotAcc; + v[6] = q_acc; + v[7] = q_acc; + v[8] = q_acc; + v[9] = q_mag; + v[10] = q_mag; + v[11] = q_mag; + memset(&Q[0], 0, 144U * sizeof(float)); + for (i = 0; i < 12; i++) { + Q[i + 12 * i] = v[i]; + } + + Q_not_empty = TRUE; + } + + /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + A_lin[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; + } + + P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; + } + } + + /* % update */ + /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0,0,0,0; */ + /* 0,0,0,r_accel,0,0,0,0,0; */ + /* 0,0,0,0,r_accel,0,0,0,0; */ + /* 0,0,0,0,0,r_accel,0,0,0; */ + /* 0,0,0,0,0,0,r_mag,0,0; */ + /* 0,0,0,0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:182' Z, Z, E, Z; */ + /* 'AttitudeEKF:183' Z, Z, Z, E]; */ + /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ + /* S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 12; i++) { + a[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + S_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ + b_r_gyro[0] = r_gyro; + b_r_gyro[1] = r_gyro; + b_r_gyro[2] = r_gyro; + b_r_gyro[3] = r_accel; + b_r_gyro[4] = r_accel; + b_r_gyro[5] = r_accel; + b_r_gyro[6] = r_mag; + b_r_gyro[7] = r_mag; + b_r_gyro[8] = r_mag; + for (r2 = 0; r2 < 9; r2++) { + b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; + } + + for (r2 = 0; r2 < 9; r2++) { + S_k[10 * r2] = b_O[r2]; + } + + /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 9; i++) { + a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + mrdivide(a, S_k, K_k); + + /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 9; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; + } + + b_O[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 9; i++) { + maxval += K_k[r2 + 12 * i] * b_O[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 9; r1++) { + maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:196' else */ + /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:200' 0,r_gyro,0; */ + /* 'AttitudeEKF:201' 0,0,r_gyro]; */ + /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ + /* observation matrix */ + /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 3; i++) { + O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ + c_r_gyro[0] = r_gyro; + c_r_gyro[1] = r_gyro; + c_r_gyro[2] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + O[r2 << 2] = b_muk[r2]; + } + + /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[r2 + 3 * i]; + } + + for (i = 0; i < 12; i++) { + B[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(b_O[0]); + a21 = (real32_T)fabs(b_O[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(b_O[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_O[r2] /= b_O[r1]; + b_O[r3] /= b_O[r1]; + b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; + b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; + b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; + b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; + if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + b_O[3 + r3] /= b_O[3 + r2]; + b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; + for (i = 0; i < 12; i++) { + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * + b_O[3 + r3]; + Y[2 + 3 * i] /= b_O[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; + Y[1 + 3 * i] /= b_O[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; + Y[3 * i] /= b_O[r1]; + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; + } + } + + /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; + } + + b_muk[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 3; i++) { + maxval += b_S_k[r2 + 12 * i] * b_muk[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:217' else */ + /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_accel,0,0; */ + /* 0,0,0,0,r_accel,0; */ + /* 0,0,0,0,0,r_accel]; */ + /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ + /* observation matrix */ + /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:231' Z, Z, E, Z]; */ + /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_accel; + d_r_gyro[4] = r_accel; + d_r_gyro[5] = r_accel; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 6; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; + } + + d_r_gyro[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:243' else */ + /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_mag,0,0; */ + /* 0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:255' Z, Z, Z, E]; */ + /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_mag; + d_r_gyro[4] = r_mag; + d_r_gyro[5] = r_mag; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2] = z[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2 + 3] = z[6 + r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = 0.0F; + for (i = 0; i < 12; i++) { + c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; + } + + b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * b_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:267' else */ + /* 'AttitudeEKF:268' x_apo=x_apr; */ + for (i = 0; i < 12; i++) { + x_apo[i] = x_apr[i]; + } + + /* 'AttitudeEKF:269' P_apo=P_apr; */ + memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + maxval = norm(*(float (*)[3])&x_apo[6]); + a21 = norm(*(float (*)[3])&x_apo[9]); + for (i = 0; i < 3; i++) { + /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + muk[i] = -x_apo[i + 6] / maxval; + zek[i] = x_apo[i + 9] / a21; + } + + /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ + wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; + wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; + wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; + + /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ + maxval = norm(wak); + for (r2 = 0; r2 < 3; r2++) { + wak[r2] /= maxval; + } + + /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ + zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; + zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; + zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; + + /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ + maxval = norm(zek); + for (r2 = 0; r2 < 3; r2++) { + zek[r2] /= maxval; + } + + /* 'AttitudeEKF:288' xa_apo=x_apo; */ + for (i = 0; i < 12; i++) { + xa_apo[i] = x_apo[i]; + } + + /* 'AttitudeEKF:289' Pa_apo=P_apo; */ + memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); + + /* rotation matrix from earth to body system */ + /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (r2 = 0; r2 < 3; r2++) { + Rot_matrix[r2] = zek[r2]; + Rot_matrix[3 + r2] = wak[r2]; + Rot_matrix[6 + r2] = muk[r2]; + } + + /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); +} + +void AttitudeEKF_initialize(void) +{ + Q_not_empty = FALSE; + Ji_not_empty = FALSE; + AttitudeEKF_init(); +} + +void AttitudeEKF_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (AttitudeEKF.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h new file mode 100644 index 0000000000..7094da1da5 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -0,0 +1,26 @@ +/* + * AttitudeEKF.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_H__ +#define __ATTITUDEEKF_H__ +/* Include files */ +#include +#include +#include +#include + +#include "rtwtypes.h" +#include "AttitudeEKF_types.h" + +/* Function Declarations */ +extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); +extern void AttitudeEKF_initialize(void); +extern void AttitudeEKF_terminate(void); +#endif +/* End of code generation (AttitudeEKF.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h new file mode 100644 index 0000000000..3f7522ffa0 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -0,0 +1,17 @@ +/* + * AttitudeEKF_types.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_TYPES_H__ +#define __ATTITUDEEKF_TYPES_H__ + +/* Include files */ +#include "rtwtypes.h" + +#endif +/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a8..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/* - * attitudeKalmanfilter.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" -#include "norm.h" -#include "cross.h" -#include "eye.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) -{ - real32_T y; - int32_T b_u0; - int32_T b_u1; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else if (rtIsInfF(u0) && rtIsInfF(u1)) { - if (u0 > 0.0F) { - b_u0 = 1; - } else { - b_u0 = -1; - } - - if (u1 > 0.0F) { - b_u1 = 1; - } else { - b_u1 = -1; - } - - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); - } else if (u1 == 0.0F) { - if (u0 > 0.0F) { - y = RT_PIF / 2.0F; - } else if (u0 < 0.0F) { - y = -(RT_PIF / 2.0F); - } else { - y = 0.0F; - } - } else { - y = (real32_T)atan2(u0, u1); - } - - return y; -} - -/* - * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) - */ -void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const - real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T - P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T - eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T - P_aposteriori[144]) -{ - real32_T wak[3]; - real32_T O[9]; - real_T dv0[9]; - real32_T a[9]; - int32_T i; - real32_T b_a[9]; - real32_T x_n_b[3]; - real32_T b_x_aposteriori_k[3]; - real32_T z_n_b[3]; - real32_T c_a[3]; - real32_T d_a[3]; - int32_T i0; - real32_T x_apriori[12]; - real_T dv1[144]; - real32_T A_lin[144]; - static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T b_A_lin[144]; - real32_T b_q[144]; - real32_T c_A_lin[144]; - real32_T d_A_lin[144]; - real32_T e_A_lin[144]; - int32_T i1; - real32_T P_apriori[144]; - real32_T b_P_apriori[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T K_k[108]; - real32_T fv0[81]; - static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - - real32_T b_r[81]; - real32_T fv1[81]; - real32_T f0; - real32_T c_P_apriori[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - - real32_T fv2[36]; - static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 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 }; - - real32_T c_r[9]; - real32_T b_K_k[36]; - real32_T d_P_apriori[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 1, 0, 0, 0 }; - - real32_T c_K_k[72]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0 }; - - real32_T b_z[6]; - static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 1 }; - - static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - - real32_T fv3[6]; - real32_T c_z[6]; - - /* Extended Attitude Kalmanfilter */ - /* */ - /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ - /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ - /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ - /* */ - /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ - /* */ - /* Example.... */ - /* */ - /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ - /* coder.varsize('udpIndVect', [9,1], [1,0]) */ - /* udpIndVect=find(updVect); */ - /* process and measurement noise covariance matrix */ - /* Q = diag(q.^2*dt); */ - /* observation matrix */ - /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ - /* % prediction section */ - /* body angular accelerations */ - /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ - wak[0] = x_aposteriori_k[3]; - wak[1] = x_aposteriori_k[4]; - wak[2] = x_aposteriori_k[5]; - - /* body angular rates */ - /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ - /* derivative of the prediction rotation matrix */ - /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ - O[0] = 0.0F; - O[1] = -x_aposteriori_k[2]; - O[2] = x_aposteriori_k[1]; - O[3] = x_aposteriori_k[2]; - O[4] = 0.0F; - O[5] = -x_aposteriori_k[0]; - O[6] = -x_aposteriori_k[1]; - O[7] = x_aposteriori_k[0]; - O[8] = 0.0F; - - /* prediction of the earth z vector */ - /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* prediction of the magnetic vector */ - /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ - eye(dv0); - for (i = 0; i < 9; i++) { - b_a[i] = (real32_T)dv0[i] + O[i] * dt; - } - - /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:66' -zez,0,zex; */ - /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:69' -muz,0,mux; */ - /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:74' E=eye(3); */ - /* 'attitudeKalmanfilter:76' Z=zeros(3); */ - /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[0]; - x_n_b[1] = x_aposteriori_k[1]; - x_n_b[2] = x_aposteriori_k[2]; - b_x_aposteriori_k[0] = x_aposteriori_k[6]; - b_x_aposteriori_k[1] = x_aposteriori_k[7]; - b_x_aposteriori_k[2] = x_aposteriori_k[8]; - z_n_b[0] = x_aposteriori_k[9]; - z_n_b[1] = x_aposteriori_k[10]; - z_n_b[2] = x_aposteriori_k[11]; - for (i = 0; i < 3; i++) { - c_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; - } - - d_a[i] = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; - } - - x_apriori[i] = x_n_b[i] + dt * wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 3] = wak[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 6] = c_a[i]; - } - - for (i = 0; i < 3; i++) { - x_apriori[i + 9] = d_a[i]; - } - - /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ - /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; - } - - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * i) + 3] = 0.0F; - } - } - - A_lin[6] = 0.0F; - A_lin[7] = x_aposteriori_k[8]; - A_lin[8] = -x_aposteriori_k[7]; - A_lin[18] = -x_aposteriori_k[8]; - A_lin[19] = 0.0F; - A_lin[20] = x_aposteriori_k[6]; - A_lin[30] = x_aposteriori_k[7]; - A_lin[31] = -x_aposteriori_k[6]; - A_lin[32] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; - } - } - - A_lin[9] = 0.0F; - A_lin[10] = x_aposteriori_k[11]; - A_lin[11] = -x_aposteriori_k[10]; - A_lin[21] = -x_aposteriori_k[11]; - A_lin[22] = 0.0F; - A_lin[23] = x_aposteriori_k[9]; - A_lin[33] = x_aposteriori_k[7]; - A_lin[34] = -x_aposteriori_k[9]; - A_lin[35] = 0.0F; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * - dt; - } - } - - /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ - /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ - /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ - /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ - /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ - /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ - /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ - b_q[0] = q[0]; - b_q[12] = 0.0F; - b_q[24] = 0.0F; - b_q[36] = 0.0F; - b_q[48] = 0.0F; - b_q[60] = 0.0F; - b_q[72] = 0.0F; - b_q[84] = 0.0F; - b_q[96] = 0.0F; - b_q[108] = 0.0F; - b_q[120] = 0.0F; - b_q[132] = 0.0F; - b_q[1] = 0.0F; - b_q[13] = q[0]; - b_q[25] = 0.0F; - b_q[37] = 0.0F; - b_q[49] = 0.0F; - b_q[61] = 0.0F; - b_q[73] = 0.0F; - b_q[85] = 0.0F; - b_q[97] = 0.0F; - b_q[109] = 0.0F; - b_q[121] = 0.0F; - b_q[133] = 0.0F; - b_q[2] = 0.0F; - b_q[14] = 0.0F; - b_q[26] = q[0]; - b_q[38] = 0.0F; - b_q[50] = 0.0F; - b_q[62] = 0.0F; - b_q[74] = 0.0F; - b_q[86] = 0.0F; - b_q[98] = 0.0F; - b_q[110] = 0.0F; - b_q[122] = 0.0F; - b_q[134] = 0.0F; - b_q[3] = 0.0F; - b_q[15] = 0.0F; - b_q[27] = 0.0F; - b_q[39] = q[1]; - b_q[51] = 0.0F; - b_q[63] = 0.0F; - b_q[75] = 0.0F; - b_q[87] = 0.0F; - b_q[99] = 0.0F; - b_q[111] = 0.0F; - b_q[123] = 0.0F; - b_q[135] = 0.0F; - b_q[4] = 0.0F; - b_q[16] = 0.0F; - b_q[28] = 0.0F; - b_q[40] = 0.0F; - b_q[52] = q[1]; - b_q[64] = 0.0F; - b_q[76] = 0.0F; - b_q[88] = 0.0F; - b_q[100] = 0.0F; - b_q[112] = 0.0F; - b_q[124] = 0.0F; - b_q[136] = 0.0F; - b_q[5] = 0.0F; - b_q[17] = 0.0F; - b_q[29] = 0.0F; - b_q[41] = 0.0F; - b_q[53] = 0.0F; - b_q[65] = q[1]; - b_q[77] = 0.0F; - b_q[89] = 0.0F; - b_q[101] = 0.0F; - b_q[113] = 0.0F; - b_q[125] = 0.0F; - b_q[137] = 0.0F; - b_q[6] = 0.0F; - b_q[18] = 0.0F; - b_q[30] = 0.0F; - b_q[42] = 0.0F; - b_q[54] = 0.0F; - b_q[66] = 0.0F; - b_q[78] = q[2]; - b_q[90] = 0.0F; - b_q[102] = 0.0F; - b_q[114] = 0.0F; - b_q[126] = 0.0F; - b_q[138] = 0.0F; - b_q[7] = 0.0F; - b_q[19] = 0.0F; - b_q[31] = 0.0F; - b_q[43] = 0.0F; - b_q[55] = 0.0F; - b_q[67] = 0.0F; - b_q[79] = 0.0F; - b_q[91] = q[2]; - b_q[103] = 0.0F; - b_q[115] = 0.0F; - b_q[127] = 0.0F; - b_q[139] = 0.0F; - b_q[8] = 0.0F; - b_q[20] = 0.0F; - b_q[32] = 0.0F; - b_q[44] = 0.0F; - b_q[56] = 0.0F; - b_q[68] = 0.0F; - b_q[80] = 0.0F; - b_q[92] = 0.0F; - b_q[104] = q[2]; - b_q[116] = 0.0F; - b_q[128] = 0.0F; - b_q[140] = 0.0F; - b_q[9] = 0.0F; - b_q[21] = 0.0F; - b_q[33] = 0.0F; - b_q[45] = 0.0F; - b_q[57] = 0.0F; - b_q[69] = 0.0F; - b_q[81] = 0.0F; - b_q[93] = 0.0F; - b_q[105] = 0.0F; - b_q[117] = q[3]; - b_q[129] = 0.0F; - b_q[141] = 0.0F; - b_q[10] = 0.0F; - b_q[22] = 0.0F; - b_q[34] = 0.0F; - b_q[46] = 0.0F; - b_q[58] = 0.0F; - b_q[70] = 0.0F; - b_q[82] = 0.0F; - b_q[94] = 0.0F; - b_q[106] = 0.0F; - b_q[118] = 0.0F; - b_q[130] = q[3]; - b_q[142] = 0.0F; - b_q[11] = 0.0F; - b_q[23] = 0.0F; - b_q[35] = 0.0F; - b_q[47] = 0.0F; - b_q[59] = 0.0F; - b_q[71] = 0.0F; - b_q[83] = 0.0F; - b_q[95] = 0.0F; - b_q[107] = 0.0F; - b_q[119] = 0.0F; - b_q[131] = 0.0F; - b_q[143] = q[3]; - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * - i0]; - } - - c_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 12; i0++) { - d_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - e_A_lin[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; - } - } - - /* % update */ - /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:112' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ - /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ - /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ - /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* [zw;ze;zmk]; */ - /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 9; i0++) { - b_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 - + 12 * i0]; - } - } - } - - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 12; i0++) { - K_k[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; - } - } - - for (i0 = 0; i0 < 9; i0++) { - fv0[i + 9 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; - } - } - } - - b_r[0] = r[0]; - b_r[9] = 0.0F; - b_r[18] = 0.0F; - b_r[27] = 0.0F; - b_r[36] = 0.0F; - b_r[45] = 0.0F; - b_r[54] = 0.0F; - b_r[63] = 0.0F; - b_r[72] = 0.0F; - b_r[1] = 0.0F; - b_r[10] = r[0]; - b_r[19] = 0.0F; - b_r[28] = 0.0F; - b_r[37] = 0.0F; - b_r[46] = 0.0F; - b_r[55] = 0.0F; - b_r[64] = 0.0F; - b_r[73] = 0.0F; - b_r[2] = 0.0F; - b_r[11] = 0.0F; - b_r[20] = r[0]; - b_r[29] = 0.0F; - b_r[38] = 0.0F; - b_r[47] = 0.0F; - b_r[56] = 0.0F; - b_r[65] = 0.0F; - b_r[74] = 0.0F; - b_r[3] = 0.0F; - b_r[12] = 0.0F; - b_r[21] = 0.0F; - b_r[30] = r[1]; - b_r[39] = 0.0F; - b_r[48] = 0.0F; - b_r[57] = 0.0F; - b_r[66] = 0.0F; - b_r[75] = 0.0F; - b_r[4] = 0.0F; - b_r[13] = 0.0F; - b_r[22] = 0.0F; - b_r[31] = 0.0F; - b_r[40] = r[1]; - b_r[49] = 0.0F; - b_r[58] = 0.0F; - b_r[67] = 0.0F; - b_r[76] = 0.0F; - b_r[5] = 0.0F; - b_r[14] = 0.0F; - b_r[23] = 0.0F; - b_r[32] = 0.0F; - b_r[41] = 0.0F; - b_r[50] = r[1]; - b_r[59] = 0.0F; - b_r[68] = 0.0F; - b_r[77] = 0.0F; - b_r[6] = 0.0F; - b_r[15] = 0.0F; - b_r[24] = 0.0F; - b_r[33] = 0.0F; - b_r[42] = 0.0F; - b_r[51] = 0.0F; - b_r[60] = r[2]; - b_r[69] = 0.0F; - b_r[78] = 0.0F; - b_r[7] = 0.0F; - b_r[16] = 0.0F; - b_r[25] = 0.0F; - b_r[34] = 0.0F; - b_r[43] = 0.0F; - b_r[52] = 0.0F; - b_r[61] = 0.0F; - b_r[70] = r[2]; - b_r[79] = 0.0F; - b_r[8] = 0.0F; - b_r[17] = 0.0F; - b_r[26] = 0.0F; - b_r[35] = 0.0F; - b_r[44] = 0.0F; - b_r[53] = 0.0F; - b_r[62] = 0.0F; - b_r[71] = 0.0F; - b_r[80] = r[2]; - for (i = 0; i < 9; i++) { - for (i0 = 0; i0 < 9; i0++) { - fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; - } - } - - mrdivide(b_P_apriori, fv1, K_k); - - /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 9; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; - } - - O[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * O[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:138' else */ - /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ - /* 'attitudeKalmanfilter:142' 0,r(1),0; */ - /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 3; i0++) { - c_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv3[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 12; i0++) { - fv2[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * - i0]; - } - } - - for (i0 = 0; i0 < 3; i0++) { - O[i + 3 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; - } - } - } - - c_r[0] = r[0]; - c_r[3] = 0.0F; - c_r[6] = 0.0F; - c_r[1] = 0.0F; - c_r[4] = r[0]; - c_r[7] = 0.0F; - c_r[2] = 0.0F; - c_r[5] = 0.0F; - c_r[8] = r[0]; - for (i = 0; i < 3; i++) { - for (i0 = 0; i0 < 3; i0++) { - a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; - } - } - - b_mrdivide(c_P_apriori, a, b_K_k); - - /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; - } - - x_n_b[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 3; i0++) { - f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 3; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:156' else */ - /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ - if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) - { - /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ - if ((z[5] < 4.0F) || (z[4] > 15.0F)) { - /* 'attitudeKalmanfilter:159' r(2)=10000; */ - r[1] = 10000.0F; - } - - /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ - /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ - /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv5[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 - * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[1]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[1]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[1]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 6; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; - } - - b_z[i] = z[i] - f0; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * b_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 - + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:181' else */ - /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ - if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) - { - /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ - /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ - /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ - /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ - /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ - /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ - /* observation matrix */ - /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 6; i0++) { - d_P_apriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv7[i1 + 12 * i0]; - } - } - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - c_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - - for (i0 = 0; i0 < 6; i0++) { - fv2[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * - i0]; - } - } - } - - b_K_k[0] = r[0]; - b_K_k[6] = 0.0F; - b_K_k[12] = 0.0F; - b_K_k[18] = 0.0F; - b_K_k[24] = 0.0F; - b_K_k[30] = 0.0F; - b_K_k[1] = 0.0F; - b_K_k[7] = r[0]; - b_K_k[13] = 0.0F; - b_K_k[19] = 0.0F; - b_K_k[25] = 0.0F; - b_K_k[31] = 0.0F; - b_K_k[2] = 0.0F; - b_K_k[8] = 0.0F; - b_K_k[14] = r[0]; - b_K_k[20] = 0.0F; - b_K_k[26] = 0.0F; - b_K_k[32] = 0.0F; - b_K_k[3] = 0.0F; - b_K_k[9] = 0.0F; - b_K_k[15] = 0.0F; - b_K_k[21] = r[2]; - b_K_k[27] = 0.0F; - b_K_k[33] = 0.0F; - b_K_k[4] = 0.0F; - b_K_k[10] = 0.0F; - b_K_k[16] = 0.0F; - b_K_k[22] = 0.0F; - b_K_k[28] = r[2]; - b_K_k[34] = 0.0F; - b_K_k[5] = 0.0F; - b_K_k[11] = 0.0F; - b_K_k[17] = 0.0F; - b_K_k[23] = 0.0F; - b_K_k[29] = 0.0F; - b_K_k[35] = r[2]; - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; - } - } - - c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); - - /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ - for (i = 0; i < 3; i++) { - b_z[i] = z[i]; - } - - for (i = 0; i < 3; i++) { - b_z[i + 3] = z[i + 6]; - } - - for (i = 0; i < 6; i++) { - fv3[i] = 0.0F; - for (i0 = 0; i0 < 12; i0++) { - fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; - } - - c_z[i] = b_z[i] - fv3[i]; - } - - for (i = 0; i < 12; i++) { - f0 = 0.0F; - for (i0 = 0; i0 < 6; i0++) { - f0 += c_K_k[i + 12 * i0] * c_z[i0]; - } - - x_aposteriori[i] = x_apriori[i] + f0; - } - - /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ - b_eye(dv1); - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 6; i1++) { - f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; - } - - b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; - } - } - - for (i = 0; i < 12; i++) { - for (i0 = 0; i0 < 12; i0++) { - P_aposteriori[i + 12 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * - P_apriori[i1 + 12 * i0]; - } - } - } - } else { - /* 'attitudeKalmanfilter:202' else */ - /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ - for (i = 0; i < 12; i++) { - x_aposteriori[i] = x_apriori[i]; - } - - /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ - memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); - } - } - } - } - - /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = -x_aposteriori[i + 6]; - } - - rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - - /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ - rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), wak); - - /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - cross(z_n_b, x_n_b, wak); - - /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ - for (i = 0; i < 3; i++) { - x_n_b[i] = wak[i]; - } - - rdivide(x_n_b, norm(wak), wak); - - /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(wak, z_n_b, x_n_b); - - /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ - for (i = 0; i < 3; i++) { - b_x_aposteriori_k[i] = x_n_b[i]; - } - - rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - - /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ - for (i = 0; i < 3; i++) { - Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = wak[i]; - Rot_matrix[6 + i] = z_n_b[i]; - } - - /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ - eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); - eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); -} - -/* End of code generation (attitudeKalmanfilter.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a91..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_H__ -#define __ATTITUDEKALMANFILTER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); -#endif -/* End of code generation (attitudeKalmanfilter.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.c - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_initialize.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_initialize(void) -{ - rt_InitInfAndNaN(8U); -} - -/* End of code generation (attitudeKalmanfilter_initialize.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be663..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_initialize.h - * - * Code generation for function 'attitudeKalmanfilter_initialize' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__ -#define __ATTITUDEKALMANFILTER_INITIALIZE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_initialize(void); -#endif -/* End of code generation (attitudeKalmanfilter_initialize.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f97274196..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.c - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "attitudeKalmanfilter_terminate.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ -void attitudeKalmanfilter_terminate(void) -{ - /* (no terminate code required) */ -} - -/* End of code generation (attitudeKalmanfilter_terminate.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a50244..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * attitudeKalmanfilter_terminate.h - * - * Code generation for function 'attitudeKalmanfilter_terminate' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__ -#define __ATTITUDEKALMANFILTER_TERMINATE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void attitudeKalmanfilter_terminate(void); -#endif -/* End of code generation (attitudeKalmanfilter_terminate.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * attitudeKalmanfilter_types.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __ATTITUDEKALMANFILTER_TYPES_H__ -#define __ATTITUDEKALMANFILTER_TYPES_H__ - -/* Type Definitions */ - -#endif -/* End of code generation (attitudeKalmanfilter_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002b..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -1,37 +0,0 @@ -/* - * cross.c - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "cross.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]) -{ - c[0] = a[1] * b[2] - a[2] * b[1]; - c[1] = a[2] * b[0] - a[0] * b[2]; - c[2] = a[0] * b[1] - a[1] * b[0]; -} - -/* End of code generation (cross.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684a..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * cross.h - * - * Code generation for function 'cross' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __CROSS_H__ -#define __CROSS_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]); -#endif -/* End of code generation (cross.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -1,51 +0,0 @@ -/* - * eye.c - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "eye.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_eye(real_T I[144]) -{ - int32_T i; - memset(&I[0], 0, 144U * sizeof(real_T)); - for (i = 0; i < 12; i++) { - I[i + 12 * i] = 1.0; - } -} - -/* - * - */ -void eye(real_T I[9]) -{ - int32_T i; - memset(&I[0], 0, 9U * sizeof(real_T)); - for (i = 0; i < 3; i++) { - I[i + 3 * i] = 1.0; - } -} - -/* End of code generation (eye.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe76717..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -1,35 +0,0 @@ -/* - * eye.h - * - * Code generation for function 'eye' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __EYE_H__ -#define __EYE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_eye(real_T I[144]); -extern void eye(real_T I[9]); -#endif -/* End of code generation (eye.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4f..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -1,357 +0,0 @@ -/* - * mrdivide.c - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "mrdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]) -{ - real32_T b_A[9]; - int32_T rtemp; - int32_T k; - real32_T b_B[36]; - int32_T r1; - int32_T r2; - int32_T r3; - real32_T maxval; - real32_T a21; - real32_T Y[36]; - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 3; k++) { - b_A[k + 3 * rtemp] = B[rtemp + 3 * k]; - } - } - - for (rtemp = 0; rtemp < 12; rtemp++) { - for (k = 0; k < 3; k++) { - b_B[k + 3 * rtemp] = A[rtemp + 12 * k]; - } - } - - r1 = 0; - r2 = 1; - r3 = 2; - maxval = (real32_T)fabs(b_A[0]); - a21 = (real32_T)fabs(b_A[1]); - if (a21 > maxval) { - maxval = a21; - r1 = 1; - r2 = 0; - } - - if ((real32_T)fabs(b_A[2]) > maxval) { - r1 = 2; - r2 = 1; - r3 = 0; - } - - b_A[r2] /= b_A[r1]; - b_A[r3] /= b_A[r1]; - b_A[3 + r2] -= b_A[r2] * b_A[3 + r1]; - b_A[3 + r3] -= b_A[r3] * b_A[3 + r1]; - b_A[6 + r2] -= b_A[r2] * b_A[6 + r1]; - b_A[6 + r3] -= b_A[r3] * b_A[6 + r1]; - if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) { - rtemp = r2; - r2 = r3; - r3 = rtemp; - } - - b_A[3 + r3] /= b_A[3 + r2]; - b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2]; - for (k = 0; k < 12; k++) { - Y[3 * k] = b_B[r1 + 3 * k]; - Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2]; - Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3 - + r3]; - Y[2 + 3 * k] /= b_A[6 + r3]; - Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1]; - Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2]; - Y[1 + 3 * k] /= b_A[3 + r2]; - Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1]; - Y[3 * k] /= b_A[r1]; - } - - for (rtemp = 0; rtemp < 3; rtemp++) { - for (k = 0; k < 12; k++) { - y[k + 12 * rtemp] = Y[rtemp + 3 * k]; - } - } -} - -/* - * - */ -void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]) -{ - real32_T b_A[36]; - int8_T ipiv[6]; - int32_T i3; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[72]; - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 6; iy++) { - b_A[iy + 6 * i3] = B[i3 + 6 * iy]; - } - - ipiv[i3] = (int8_T)(1 + i3); - } - - for (j = 0; j < 5; j++) { - c = j * 7; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 6 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 6; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 6; - iy += 6; - } - } - - i3 = (c - j) + 6; - for (jy = c + 1; jy + 1 <= i3; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 6; - for (k = 1; k <= 5 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i3 = (iy - j) + 12; - for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 6; - iy += 6; - } - } - - for (i3 = 0; i3 < 12; i3++) { - for (iy = 0; iy < 6; iy++) { - Y[iy + 6 * i3] = A[i3 + 12 * iy]; - } - } - - for (jy = 0; jy < 6; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 6 * j]; - Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; - Y[(ipiv[jy] + 6 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 0; k < 6; k++) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 7; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 6 * j; - for (k = 5; k > -1; k += -1) { - iy = 6 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i3 = 0; i3 < 6; i3++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i3] = Y[i3 + 6 * iy]; - } - } -} - -/* - * - */ -void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]) -{ - real32_T b_A[81]; - int8_T ipiv[9]; - int32_T i2; - int32_T iy; - int32_T j; - int32_T c; - int32_T ix; - real32_T temp; - int32_T k; - real32_T s; - int32_T jy; - int32_T ijA; - real32_T Y[108]; - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 9; iy++) { - b_A[iy + 9 * i2] = B[i2 + 9 * iy]; - } - - ipiv[i2] = (int8_T)(1 + i2); - } - - for (j = 0; j < 8; j++) { - c = j * 10; - iy = 0; - ix = c; - temp = (real32_T)fabs(b_A[c]); - for (k = 2; k <= 9 - j; k++) { - ix++; - s = (real32_T)fabs(b_A[ix]); - if (s > temp) { - iy = k - 1; - temp = s; - } - } - - if (b_A[c + iy] != 0.0F) { - if (iy != 0) { - ipiv[j] = (int8_T)((j + iy) + 1); - ix = j; - iy += j; - for (k = 0; k < 9; k++) { - temp = b_A[ix]; - b_A[ix] = b_A[iy]; - b_A[iy] = temp; - ix += 9; - iy += 9; - } - } - - i2 = (c - j) + 9; - for (jy = c + 1; jy + 1 <= i2; jy++) { - b_A[jy] /= b_A[c]; - } - } - - iy = c; - jy = c + 9; - for (k = 1; k <= 8 - j; k++) { - temp = b_A[jy]; - if (b_A[jy] != 0.0F) { - ix = c + 1; - i2 = (iy - j) + 18; - for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) { - b_A[ijA] += b_A[ix] * -temp; - ix++; - } - } - - jy += 9; - iy += 9; - } - } - - for (i2 = 0; i2 < 12; i2++) { - for (iy = 0; iy < 9; iy++) { - Y[iy + 9 * i2] = A[i2 + 12 * iy]; - } - } - - for (jy = 0; jy < 9; jy++) { - if (ipiv[jy] != jy + 1) { - for (j = 0; j < 12; j++) { - temp = Y[jy + 9 * j]; - Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; - Y[(ipiv[jy] + 9 * j) - 1] = temp; - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 0; k < 9; k++) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - for (jy = k + 2; jy < 10; jy++) { - Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; - } - } - } - } - - for (j = 0; j < 12; j++) { - c = 9 * j; - for (k = 8; k > -1; k += -1) { - iy = 9 * k; - if (Y[k + c] != 0.0F) { - Y[k + c] /= b_A[k + iy]; - for (jy = 0; jy + 1 <= k; jy++) { - Y[jy + c] -= Y[k + c] * b_A[jy + iy]; - } - } - } - } - - for (i2 = 0; i2 < 9; i2++) { - for (iy = 0; iy < 12; iy++) { - y[iy + 12 * i2] = Y[i2 + 9 * iy]; - } - } -} - -/* End of code generation (mrdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51ff..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -1,36 +0,0 @@ -/* - * mrdivide.h - * - * Code generation for function 'mrdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __MRDIVIDE_H__ -#define __MRDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]); -extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]); -extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]); -#endif -/* End of code generation (mrdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -1,54 +0,0 @@ -/* - * norm.c - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "norm.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -real32_T norm(const real32_T x[3]) -{ - real32_T y; - real32_T scale; - int32_T k; - real32_T absxk; - real32_T t; - y = 0.0F; - scale = 1.17549435E-38F; - for (k = 0; k < 3; k++) { - absxk = (real32_T)fabs(x[k]); - if (absxk > scale) { - t = scale / absxk; - y = 1.0F + y * t * t; - scale = absxk; - } else { - t = absxk / scale; - y += t * t; - } - } - - return scale * (real32_T)sqrt(y); -} - -/* End of code generation (norm.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b571..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * norm.h - * - * Code generation for function 'norm' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __NORM_H__ -#define __NORM_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern real32_T norm(const real32_T x[3]); -#endif -/* End of code generation (norm.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e1..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * rdivide.c - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "rdivide.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void rdivide(const real32_T x[3], real32_T y, real32_T z[3]) -{ - int32_T i; - for (i = 0; i < 3; i++) { - z[i] = x[i] / y; - } -} - -/* End of code generation (rdivide.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe28..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * rdivide.h - * - * Code generation for function 'rdivide' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RDIVIDE_H__ -#define __RDIVIDE_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]); -#endif -/* End of code generation (rdivide.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d1044..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/* - * rtGetInf.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, Inf and MinusInf - */ -#include "rtGetInf.h" -#define NumBitsPerChar 8U - -/* Function: rtGetInf ================================================== - * Abstract: - * Initialize rtInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T inf = 0.0; - if (bitsPerReal == 32U) { - inf = rtGetInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - inf = tmpVal.fltVal; - break; - } - } - } - - return inf; -} - -/* Function: rtGetInfF ================================================== - * Abstract: - * Initialize rtInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetInfF(void) -{ - IEEESingle infF; - infF.wordL.wordLuint = 0x7F800000U; - return infF.wordL.wordLreal; -} - -/* Function: rtGetMinusInf ================================================== - * Abstract: - * Initialize rtMinusInf needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetMinusInf(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T minf = 0.0; - if (bitsPerReal == 32U) { - minf = rtGetMinusInfF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF00000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - minf = tmpVal.fltVal; - break; - } - } - } - - return minf; -} - -/* Function: rtGetMinusInfF ================================================== - * Abstract: - * Initialize rtMinusInfF needed by the generated code. - * Inf is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetMinusInfF(void) -{ - IEEESingle minfF; - minfF.wordL.wordLuint = 0xFF800000U; - return minfF.wordL.wordLreal; -} - -/* End of code generation (rtGetInf.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd02..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/* - * rtGetInf.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETINF_H__ -#define __RTGETINF_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetInf(void); -extern real32_T rtGetInfF(void); -extern real_T rtGetMinusInf(void); -extern real32_T rtGetMinusInfF(void); - -#endif -/* End of code generation (rtGetInf.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/* - * rtGetNaN.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finite, NaN - */ -#include "rtGetNaN.h" -#define NumBitsPerChar 8U - -/* Function: rtGetNaN ================================================== - * Abstract: - * Initialize rtNaN needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real_T rtGetNaN(void) -{ - size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar); - real_T nan = 0.0; - if (bitsPerReal == 32U) { - nan = rtGetNaNF(); - } else { - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - union { - LittleEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0xFFF80000U; - tmpVal.bitVal.words.wordL = 0x00000000U; - nan = tmpVal.fltVal; - break; - } - - case BigEndian: - { - union { - BigEndianIEEEDouble bitVal; - real_T fltVal; - } tmpVal; - - tmpVal.bitVal.words.wordH = 0x7FFFFFFFU; - tmpVal.bitVal.words.wordL = 0xFFFFFFFFU; - nan = tmpVal.fltVal; - break; - } - } - } - - return nan; -} - -/* Function: rtGetNaNF ================================================== - * Abstract: - * Initialize rtNaNF needed by the generated code. - * NaN is initialized as non-signaling. Assumes IEEE. - */ -real32_T rtGetNaNF(void) -{ - IEEESingle nanF = { { 0 } }; - uint16_T one = 1U; - enum { - LittleEndian, - BigEndian - } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian; - switch (machByteOrder) { - case LittleEndian: - { - nanF.wordL.wordLuint = 0xFFC00000U; - break; - } - - case BigEndian: - { - nanF.wordL.wordLuint = 0x7FFFFFFFU; - break; - } - } - - return nanF.wordL.wordLreal; -} - -/* End of code generation (rtGetNaN.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f6..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * rtGetNaN.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTGETNAN_H__ -#define __RTGETNAN_H__ - -#include -#include "rtwtypes.h" -#include "rt_nonfinite.h" - -extern real_T rtGetNaN(void); -extern real32_T rtGetNaNF(void); - -#endif -/* End of code generation (rtGetNaN.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 3564983630..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * rt_defines.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_DEFINES_H__ -#define __RT_DEFINES_H__ - -#include - -#define RT_PI 3.14159265358979323846 -#define RT_PIF 3.1415927F -#define RT_LN_10 2.30258509299404568402 -#define RT_LN_10F 2.3025851F -#define RT_LOG10E 0.43429448190325182765 -#define RT_LOG10EF 0.43429449F -#define RT_E 2.7182818284590452354 -#define RT_EF 2.7182817F -#endif -/* End of code generation (rt_defines.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2e..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/* - * rt_nonfinite.c - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -/* - * Abstract: - * MATLAB for code generation function to initialize non-finites, - * (Inf, NaN and -Inf). - */ -#include "rt_nonfinite.h" -#include "rtGetNaN.h" -#include "rtGetInf.h" - -real_T rtInf; -real_T rtMinusInf; -real_T rtNaN; -real32_T rtInfF; -real32_T rtMinusInfF; -real32_T rtNaNF; - -/* Function: rt_InitInfAndNaN ================================================== - * Abstract: - * Initialize the rtInf, rtMinusInf, and rtNaN needed by the - * generated code. NaN is initialized as non-signaling. Assumes IEEE. - */ -void rt_InitInfAndNaN(size_t realSize) -{ - (void) (realSize); - rtNaN = rtGetNaN(); - rtNaNF = rtGetNaNF(); - rtInf = rtGetInf(); - rtInfF = rtGetInfF(); - rtMinusInf = rtGetMinusInf(); - rtMinusInfF = rtGetMinusInfF(); -} - -/* Function: rtIsInf ================================================== - * Abstract: - * Test if value is infinite - */ -boolean_T rtIsInf(real_T value) -{ - return ((value==rtInf || value==rtMinusInf) ? 1U : 0U); -} - -/* Function: rtIsInfF ================================================= - * Abstract: - * Test if single-precision value is infinite - */ -boolean_T rtIsInfF(real32_T value) -{ - return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U); -} - -/* Function: rtIsNaN ================================================== - * Abstract: - * Test if value is not a number - */ -boolean_T rtIsNaN(real_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan(value)? TRUE:FALSE; -#else - return (value!=value)? 1U:0U; -#endif -} - -/* Function: rtIsNaNF ================================================= - * Abstract: - * Test if single-precision value is not a number - */ -boolean_T rtIsNaNF(real32_T value) -{ -#if defined(_MSC_VER) && (_MSC_VER <= 1200) - return _isnan((real_T)value)? true:false; -#else - return (value!=value)? 1U:0U; -#endif -} - - -/* End of code generation (rt_nonfinite.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d94..0000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * rt_nonfinite.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RT_NONFINITE_H__ -#define __RT_NONFINITE_H__ - -#if defined(_MSC_VER) && (_MSC_VER <= 1200) -#include -#endif -#include -#include "rtwtypes.h" - -extern real_T rtInf; -extern real_T rtMinusInf; -extern real_T rtNaN; -extern real32_T rtInfF; -extern real32_T rtMinusInfF; -extern real32_T rtNaNF; -extern void rt_InitInfAndNaN(size_t realSize); -extern boolean_T rtIsInf(real_T value); -extern boolean_T rtIsInfF(real32_T value); -extern boolean_T rtIsNaN(real_T value); -extern boolean_T rtIsNaNF(real32_T value); - -typedef struct { - struct { - uint32_T wordH; - uint32_T wordL; - } words; -} BigEndianIEEEDouble; - -typedef struct { - struct { - uint32_T wordL; - uint32_T wordH; - } words; -} LittleEndianIEEEDouble; - -typedef struct { - union { - real32_T wordLreal; - uint32_T wordLuint; - } wordL; -} IEEESingle; - -#endif -/* End of code generation (rt_nonfinite.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100755 new mode 100644 index 9a5c96267e..b5a02a7a6d --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -1,159 +1,160 @@ -/* - * rtwtypes.h - * - * Code generation for function 'attitudeKalmanfilter' - * - * C source code generated on: Sat Jan 19 15:25:29 2013 - * - */ - -#ifndef __RTWTYPES_H__ -#define __RTWTYPES_H__ -#ifndef TRUE -# define TRUE (1U) -#endif -#ifndef FALSE -# define FALSE (0U) -#endif -#ifndef __TMWTYPES__ -#define __TMWTYPES__ - -#include - -/*=======================================================================* - * Target hardware information - * Device type: Generic->MATLAB Host Computer - * Number of bits: char: 8 short: 16 int: 32 - * long: 32 native word size: 32 - * Byte ordering: LittleEndian - * Signed integer division rounds to: Zero - * Shift right on a signed integer as arithmetic shift: on - *=======================================================================*/ - -/*=======================================================================* - * Fixed width word size data types: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - * real32_T, real64_T - 32 and 64 bit floating point numbers * - *=======================================================================*/ - -typedef signed char int8_T; -typedef unsigned char uint8_T; -typedef short int16_T; -typedef unsigned short uint16_T; -typedef int int32_T; -typedef unsigned int uint32_T; -typedef float real32_T; -typedef double real64_T; - -/*===========================================================================* - * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * - * ulong_T, char_T and byte_T. * - *===========================================================================*/ - -typedef double real_T; -typedef double time_T; -typedef unsigned char boolean_T; -typedef int int_T; -typedef unsigned int uint_T; -typedef unsigned long ulong_T; -typedef char char_T; -typedef char_T byte_T; - -/*===========================================================================* - * Complex number type definitions * - *===========================================================================*/ -#define CREAL_T - typedef struct { - real32_T re; - real32_T im; - } creal32_T; - - typedef struct { - real64_T re; - real64_T im; - } creal64_T; - - typedef struct { - real_T re; - real_T im; - } creal_T; - - typedef struct { - int8_T re; - int8_T im; - } cint8_T; - - typedef struct { - uint8_T re; - uint8_T im; - } cuint8_T; - - typedef struct { - int16_T re; - int16_T im; - } cint16_T; - - typedef struct { - uint16_T re; - uint16_T im; - } cuint16_T; - - typedef struct { - int32_T re; - int32_T im; - } cint32_T; - - typedef struct { - uint32_T re; - uint32_T im; - } cuint32_T; - - -/*=======================================================================* - * Min and Max: * - * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * - * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * - *=======================================================================*/ - -#define MAX_int8_T ((int8_T)(127)) -#define MIN_int8_T ((int8_T)(-128)) -#define MAX_uint8_T ((uint8_T)(255)) -#define MIN_uint8_T ((uint8_T)(0)) -#define MAX_int16_T ((int16_T)(32767)) -#define MIN_int16_T ((int16_T)(-32768)) -#define MAX_uint16_T ((uint16_T)(65535)) -#define MIN_uint16_T ((uint16_T)(0)) -#define MAX_int32_T ((int32_T)(2147483647)) -#define MIN_int32_T ((int32_T)(-2147483647-1)) -#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) -#define MIN_uint32_T ((uint32_T)(0)) - -/* Logical type definitions */ -#if !defined(__cplusplus) && !defined(__true_false_are_keywords) -# ifndef false -# define false (0U) -# endif -# ifndef true -# define true (1U) -# endif -#endif - -/* - * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation - * for signed integer values. - */ -#if ((SCHAR_MIN + 1) != -SCHAR_MAX) -#error "This code must be compiled using a 2's complement representation for signed integer values" -#endif - -/* - * Maximum length of a MATLAB identifier (function/variable) - * including the null-termination character. Referenced by - * rt_logging.c and rt_matrx.c. - */ -#define TMW_NAME_LENGTH_MAX 64 - -#endif -#endif -/* End of code generation (rtwtypes.h) */ +/* + * rtwtypes.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 99d0c5bf28..749b0a91c3 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -39,16 +39,6 @@ MODULE_COMMAND = attitude_estimator_ekf SRCS = attitude_estimator_ekf_main.cpp \ attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c + codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index 31c9157e11..e0bcbc6e9e 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -223,7 +223,7 @@ BottleDrop::start() _main_task = task_spawn_cmd("bottle_drop", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 15, - 2048, + 1500, (main_t)&BottleDrop::task_main_trampoline, nullptr); @@ -283,7 +283,6 @@ BottleDrop::drop() // force the door open if we have to if (_doors_opened == 0) { open_bay(); - warnx("bay not ready, forced open"); } while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { @@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { open_bay(); drop(); - mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + mavlink_log_critical(_mavlink_fd, "drop bottle"); } else if (cmd->param1 > 0.5f) { open_bay(); - mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + mavlink_log_critical(_mavlink_fd, "opening bay"); } else { lock_release(); close_bay(); - mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + mavlink_log_critical(_mavlink_fd, "closing bay"); } answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd) switch ((int)(cmd->param1 + 0.5f)) { case 0: _drop_approval = false; - mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + mavlink_log_critical(_mavlink_fd, "got drop position, no approval"); break; case 1: _drop_approval = true; - mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + mavlink_log_critical(_mavlink_fd, "got drop position and approval"); break; default: @@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command); break; default: diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk index 6b18fff556..df9f5473b0 100644 --- a/src/modules/bottle_drop/module.mk +++ b/src/modules/bottle_drop/module.mk @@ -39,3 +39,7 @@ MODULE_COMMAND = bottle_drop SRCS = bottle_drop.cpp \ bottle_drop_params.c + +MAXOPTIMIZATION = -Os + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 0cb41489f8..d4cd97be6a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float 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] = { "front", "back", "left", "right", "top", "bottom" }; + const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float /* inform user which axes are still needed */ mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", - (!data_collected[0]) ? "front " : "", - (!data_collected[1]) ? "back " : "", + (!data_collected[5]) ? "down " : "", + (!data_collected[0]) ? "back " : "", + (!data_collected[1]) ? "front " : "", (!data_collected[2]) ? "left " : "", (!data_collected[3]) ? "right " : "", - (!data_collected[4]) ? "up " : "", - (!data_collected[5]) ? "down " : ""); + (!data_collected[4]) ? "up " : ""); /* allow user enough time to read the message */ sleep(3); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 46caddd469..c9c8d16b50 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -312,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } @@ -760,7 +762,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1543,7 +1548,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } @@ -1644,8 +1649,9 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } @@ -1848,7 +1854,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { + mavlink_log_critical(mavlink_fd, "failsafe mode off"); + } failsafe_old = status.failsafe; } @@ -2196,6 +2206,59 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: + case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_ACRO: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2208,18 +2271,46 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; + + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + case NAVIGATION_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -2275,73 +2366,6 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; } - - break; - - case NAVIGATION_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - case NAVIGATION_STATE_AUTO_LOITER: - case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTGS: - case NAVIGATION_STATE_AUTO_LANDENGFAIL: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_LANDGPSFAIL: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_LAND: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_termination_enabled = false; - break; - - case NAVIGATION_STATE_TERMINATION: - /* disable all controllers on termination */ - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_termination_enabled = true; break; default: diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9568752aed..465f9cdc52 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, * Check failsafe and main status and set navigation status for navigator accordingly */ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, - const bool stay_in_failsafe) + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; @@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_MISSION: + /* go into failsafe * - if commanded to do so * - if we have an engine failure - * - if either the datalink is enabled and lost as well as RC is lost - * - if there is no datalink and the mission is finished */ + * - depending on datalink, RC and if the mission is finished */ + + /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { @@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->gps_failure_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX - /* Finished handling commands which have priority , now handle failures */ + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; + + /* finished handling commands which have priority, now handle failures */ } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || - (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + + /* datalink loss enabled: + * check for datalink lost: this should always trigger RTGS */ + } else if (data_link_loss_enabled && status->data_link_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* also go into failsafe if just datalink is lost */ - } else if (status->data_link_lost && data_link_loss_enabled) { + /* datalink loss disabled: + * check if both, RC and datalink are lost during the mission + * or RC is lost after the mission is finished: this should always trigger RCRECOVER */ + } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) || + (status->rc_signal_lost && mission_finished))) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_TERMINATION; } - /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost && !stay_in_failsafe) { - - /* this mode is ok, we don't need RC for missions */ - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + /* stay where you are if you should stay in failsafe, otherwise everything is perfect */ } else if (!stay_in_failsafe){ - /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; @@ -703,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) { - mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE"); // XXX do not make this fatal yet } } diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index f0139a4b70..2860d1efce 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -39,3 +39,5 @@ SRCS = test_params.c \ block/BlockParam.cpp \ uorb/blocks.cpp \ blocks.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b2355d4d89..705e54be32 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -629,9 +629,6 @@ task_main(int argc, char *argv[]) { work_q_item_t *work; - /* inform about start */ - warnx("Initializing.."); - /* Initialize global variables */ g_key_offsets[0] = 0; @@ -694,16 +691,15 @@ task_main(int argc, char *argv[]) if (sys_restart_val == DM_INIT_REASON_POWER_ON) { warnx("Power on restart"); _restart(DM_INIT_REASON_POWER_ON); - } - else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { + } else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) { warnx("In flight restart"); _restart(DM_INIT_REASON_IN_FLIGHT); - } - else + } else { warnx("Unknown restart"); - } - else + } + } else { warnx("Unknown restart"); + } /* We use two file descriptors, one for the caller context and one for the worker thread */ /* They are actually the same but we need to some way to reject caller request while the */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 685f5e12f1..e7805daa91 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state() warn_index = max_warn_index; } - warnx("reset: %s", feedback[warn_index]); - mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + // Do not warn about accel offset if we have no position updates + if (!(warn_index == 5 && _ekf->staticMode)) { + warnx("reset: %s", feedback[warn_index]); + mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); + } } struct estimator_status_report rep; @@ -1557,7 +1560,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 5000, + 7500, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c251dd3b2c..29b7ec7b74 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1405,7 +1405,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 5.0f); + configure_stream("OPTICAL_FLOW_RAD", 5.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2900, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b4911427f6..a8f956ad05 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); } @@ -1719,7 +1719,53 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } + + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } @@ -1788,33 +1834,32 @@ protected: } }; - -class MavlinkStreamOpticalFlow : public MavlinkStream +class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: const char *get_name() const { - return MavlinkStreamOpticalFlow::get_name_static(); + return MavlinkStreamOpticalFlowRad::get_name_static(); } static const char *get_name_static() { - return "OPTICAL_FLOW"; + return "OPTICAL_FLOW_RAD"; } uint8_t get_id() { - return MAVLINK_MSG_ID_OPTICAL_FLOW; + return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } static MavlinkStream *new_instance(Mavlink *mavlink) { - return new MavlinkStreamOpticalFlow(mavlink); + return new MavlinkStreamOpticalFlowRad(mavlink); } unsigned get_size() { - return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1822,11 +1867,11 @@ private: uint64_t _flow_time; /* do not allow top copying this class */ - MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &); - MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &); + MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: - explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink), + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), _flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))), _flow_time(0) {} @@ -1836,18 +1881,23 @@ protected: struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_t msg; + mavlink_optical_flow_rad_t msg; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; - msg.flow_x = flow.flow_raw_x; - msg.flow_y = flow.flow_raw_y; - msg.flow_comp_m_x = flow.flow_comp_x_m; - msg.flow_comp_m_y = flow.flow_comp_y_m; + msg.integrated_x = flow.pixel_flow_x_integral; + msg.integrated_y = flow.pixel_flow_y_integral; + msg.integrated_xgyro = flow.gyro_x_rate_integral; + msg.integrated_ygyro = flow.gyro_y_rate_integral; + msg.integrated_zgyro = flow.gyro_z_rate_integral; + msg.distance = flow.ground_distance_m; msg.quality = flow.quality; - msg.ground_distance = flow.ground_distance_m; + msg.integration_time_us = flow.integration_timespan; + msg.sensor_id = flow.sensor_id; + msg.time_delta_distance_us = flow.time_since_last_sonar_update; + msg.temperature = flow.gyro_temperature; - _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg); + _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg); } } }; @@ -2153,7 +2203,7 @@ StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static), new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static), new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static), - new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static), + new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static), new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static), new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static), new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc092c7e9c..e98d72afe7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -144,8 +144,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_int(msg); break; - case MAVLINK_MSG_ID_OPTICAL_FLOW: - handle_message_optical_flow(msg); + case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD: + handle_message_optical_flow_rad(msg); break; case MAVLINK_MSG_ID_SET_MODE: @@ -352,24 +352,27 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } void -MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); + mavlink_optical_flow_rad_t flow; + mavlink_msg_optical_flow_rad_decode(msg, &flow); struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; + f.timestamp = flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; + f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -389,13 +392,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) struct optical_flow_s f; memset(&f, 0, sizeof(f)); - f.timestamp = hrt_absolute_time(); - f.flow_timestamp = flow.time_usec; - f.flow_raw_x = flow.integrated_x; - f.flow_raw_y = flow.integrated_y; + f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; + f.integration_timespan = flow.integration_time_us; + f.pixel_flow_x_integral = flow.integrated_x; + f.pixel_flow_y_integral = flow.integrated_y; + f.gyro_x_rate_integral = flow.integrated_xgyro; + f.gyro_y_rate_integral = flow.integrated_ygyro; + f.gyro_z_rate_integral = flow.integrated_zgyro; + f.time_since_last_sonar_update = flow.time_delta_distance_us; f.ground_distance_m = flow.distance; f.quality = flow.quality; f.sensor_id = flow.sensor_id; + f.gyro_temperature = flow.temperature; if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -538,12 +546,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - OFB_IGN_BIT_YAW; + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - OFB_IGN_BIT_YAWRATE; + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e5f2c6a73c..a057074a78 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,7 +112,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); void handle_message_command_int(mavlink_message_t *msg); - void handle_message_optical_flow(mavlink_message_t *msg); + void handle_message_optical_flow_rad(mavlink_message_t *msg); void handle_message_hil_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 16a3b627fd..887a535088 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -458,8 +458,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) void MulticopterAttitudeControl::task_main() { - warnx("started"); - fflush(stdout); /* * do subscriptions diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522c..5918d6bc5f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp() - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp() if (_reset_alt_sp) { _reset_alt_sp = false; _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); + mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt) void MulticopterPositionControl::task_main() { - warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 66f1c8c731..e789fd10de 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item() /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -188,6 +189,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { if (!_param_skipcommshold.get()) { @@ -208,6 +210,7 @@ DataLinkLoss::advance_dll() _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: _dll_state = DLL_STATE_TERMINATE; @@ -215,6 +218,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: warnx("dll end"); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7fac69a611..0765e9b7c0 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -371,7 +371,7 @@ Mission::set_mission_items() } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished"); /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); @@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { - /* mission item index out of bounds */ - return false; - } - - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after + * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ for (int i = 0; i < 10; i++) { + + if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { + /* mission item index out of bounds */ + return false; + } + const ssize_t len = sizeof(struct mission_item_s); /* read mission item to temp storage first to not overwrite current mission item if data damaged */ @@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (is_current) { (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) { + if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, + &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the * dataman */ mavlink_log_critical(_navigator->get_mavlink_fd(), - "ERROR DO JUMP waypoint could not be written"); + "ERROR DO JUMP waypoint could not be written"); return false; } } @@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_index_ptr = mission_item_tmp.do_jump_mission_index; } else { - mavlink_log_critical(_navigator->get_mavlink_fd(), - "DO JUMP repetitions completed"); + if (is_current) { + mavlink_log_critical(_navigator->get_mavlink_fd(), + "DO JUMP repetitions completed"); + } /* no more DO_JUMPS, therefore just try to continue with next mission item */ (*mission_index_ptr)++; } @@ -707,6 +712,7 @@ Mission::set_mission_item_reached() _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; _navigator->publish_mission_result(); + reset_mission_item_reached(); } void diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d550dcc4cb..9cd609955b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -205,6 +205,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ + bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a867dd0daf..df620e5e7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -137,6 +137,7 @@ Navigator::Navigator() : _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), + _pos_sp_triplet_published_invalid_once(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -289,8 +290,9 @@ Navigator::task_main() navigation_capabilities_update(); params_update(); - /* rate limit position updates to 50 Hz */ + /* rate limit position and sensor updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); + orb_set_interval(_sensor_combined_sub, 20); hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -426,12 +428,15 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RCRECOVER: + _pos_sp_triplet_published_invalid_once = false; if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { @@ -439,11 +444,13 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ + _pos_sp_triplet_published_invalid_once = false; if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { @@ -451,9 +458,11 @@ Navigator::task_main() } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; default: @@ -467,9 +476,9 @@ Navigator::task_main() _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } - /* if nothing is running, set position setpoint triplet invalid */ - if (_navigation_mode == nullptr) { - // TODO publish empty sp only once + /* if nothing is running, set position setpoint triplet invalid once */ + if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { + _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; @@ -497,8 +506,8 @@ Navigator::start() /* start the task */ _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, + SCHED_PRIORITY_DEFAULT + 20, + 1800, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -630,9 +639,6 @@ Navigator::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } void diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 5564a1c42e..42392e7399 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -163,6 +163,7 @@ RCLoss::advance_rcl() _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); } break; case RCL_STATE_LOITER: @@ -171,6 +172,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); + reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: warnx("rcl end"); 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 81bbaa6587..296919c042 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel @@ -298,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; - hrt_abstime flow_prev = 0; // time of last flow measurement + //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) @@ -389,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -491,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ - float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; - flow_prev = flow.flow_timestamp; +// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; +// flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && @@ -520,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -550,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //todo check direction of x und y axis + flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -721,8 +720,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ad60ee03e0..58c9429b6f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -70,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) uint8_t *bytes; *dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes); if (*dsm_updated) { - r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; @@ -91,6 +91,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated) for (unsigned i = 0; i < n_bytes; i++) { /* set updated flag if one complete packet was parsed */ + st24_rssi = RC_INPUT_RSSI_MAX; *st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count, &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS)); } @@ -170,6 +171,12 @@ controls_tick() { perf_begin(c_gather_dsm); bool dsm_updated, st24_updated; (void)dsm_port_input(&rssi, &dsm_updated, &st24_updated); + if (dsm_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + } + if (st24_updated) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -417,6 +424,15 @@ controls_tick() { if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) override = true; + /* + if the FMU is dead then enable override if we have a + mixer and OVERRIDE_IMMEDIATE is set + */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + override = true; + if (override) { r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index c0b8ac358c..66f0969de1 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0; int mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while safety off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { + /* do not allow a mixer change while safety off and FMU armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } - /* abort if we're in the mixer */ + /* disable mixing, will be enabled once load is complete */ + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK); + + /* abort if we're in the mixer - the caller is expected to retry */ if (in_mixer) { return 1; } @@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); - if (length < sizeof(px4io_mixdata)) + if (length < sizeof(px4io_mixdata)) { return 0; + } - unsigned text_length = length - sizeof(px4io_mixdata); + unsigned text_length = length - sizeof(px4io_mixdata); switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); - /* FIRST mark the mixer as invalid */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; @@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* disable mixing during the update */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 9b2e047cbb..c7e9ae3eb3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,7 @@ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ +#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE (1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 30f32b38e7..14ee9cb405 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in; #define NUM_MSG 2 static char msg[NUM_MSG][40]; +static void heartbeat_blink(void); +static void ring_blink(void); + /* * add a debug message to be printed on the console */ @@ -124,8 +127,64 @@ heartbeat_blink(void) { static bool heartbeat = false; LED_BLUE(heartbeat = !heartbeat); +} + +static void +ring_blink(void) +{ #ifdef GPIO_LED4 - LED_RING(heartbeat); + + if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + LED_RING(1); + return; + } + + // XXX this led code does have + // intentionally a few magic numbers. + const unsigned max_brightness = 118; + + static unsigned counter = 0; + static unsigned brightness = max_brightness; + static unsigned brightness_counter = 0; + static unsigned on_counter = 0; + + if (brightness_counter < max_brightness) { + + bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + + // XXX once led is PWM driven, + // remove the ! in the line below + // to return to the proper breathe + // animation / pattern (currently inverted) + LED_RING(!on); + brightness_counter++; + + if (on) { + on_counter++; + } + + } else { + + if (counter >= 62) { + counter = 0; + } + + int n; + + if (counter < 32) { + n = counter; + + } else { + n = 62 - counter; + } + + brightness = (n * n) / 8; + brightness_counter = 0; + on_counter = 0; + counter++; + } + #endif } @@ -300,6 +359,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + ring_blink(); + check_reboot(); /* check for debug activity (default: none) */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 8186e4c78b..93a33490fa 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit; #define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) #define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, !(_s)) +#define LED_RING(_s) stm32_gpiowrite(GPIO_LED4, (_s)) #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 49c2a9f56b..f0c2cfd26d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -191,7 +191,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -406,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - return mixer_handle_text(values, num_values * sizeof(*values)); - } - break; + /* do not change the mixer if FMU is armed and IO's safety is off + * this state defines an active system. This check is done in the + * text handling function. + */ + return mixer_handle_text(values, num_values * sizeof(*values)); default: /* avoid offset wrap */ @@ -582,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_REBOOT_BL: - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { // don't allow reboot while armed break; } @@ -629,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { /** - * do not allow a RC config change while outputs armed + * do not allow a RC config change while safety is off */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { break; } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index a28d43e725..d4a00af39d 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -43,3 +43,5 @@ SRCS = sdlog2.c \ logbuffer.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index af580f1f79..0b6861d2a5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -496,6 +496,8 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); + perf_counter_t perf_write = perf_alloc(PC_ELAPSED, "sd write"); + int log_fd = open_log_file(); if (log_fd < 0) { @@ -553,7 +555,9 @@ static void *logwriter_thread(void *arg) n = available; } + perf_begin(perf_write); n = write(log_fd, read_ptr, n); + perf_end(perf_write); should_wait = (n == available) && !is_part; @@ -586,6 +590,9 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); + /* free performance counter */ + perf_free(perf_write); + return NULL; } @@ -1511,11 +1518,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { log_msg.msg_type = LOG_FLOW_MSG; - log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x; - log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y; - log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m; - log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m; - log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature; + log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral; + log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral; + log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral; + log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan; + log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral; + log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral; log_msg.body.log_FLOW.quality = buf.flow.quality; log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; LOGBUFFER_WRITE_AND_COUNT(FLOW); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fa9bdacb80..30491036a1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -200,13 +200,19 @@ struct log_ARSP_s { /* --- FLOW - OPTICAL FLOW --- */ #define LOG_FLOW_MSG 15 struct log_FLOW_s { - int16_t flow_raw_x; - int16_t flow_raw_y; - float flow_comp_x; - float flow_comp_y; - float distance; - uint8_t quality; + uint64_t timestamp; uint8_t sensor_id; + float pixel_flow_x_integral; + float pixel_flow_y_integral; + float gyro_x_rate_integral; + float gyro_y_rate_integral; + float gyro_z_rate_integral; + float ground_distance_m; + uint32_t integration_timespan; + uint32_t time_since_last_sonar_update; + uint16_t frame_count_since_last_readout; + int16_t gyro_temperature; + uint8_t quality; }; /* --- GPOS - GLOBAL POSITION ESTIMATE --- */ diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index 5b1bc5e86f..dfbba92d91 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -42,3 +42,5 @@ SRCS = sensors.cpp \ sensor_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c new file mode 100644 index 0000000000..4bcf957848 --- /dev/null +++ b/src/modules/systemlib/mcu_version.c @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 mcu_version.c + * + * Read out the microcontroller version from the board + * + * @author Lorenz Meier + * + */ + +#include "mcu_version.h" + +#include + +#ifdef CONFIG_ARCH_CHIP_STM32 +#include + +#define DBGMCU_IDCODE 0xE0042000 + +#define STM32F40x_41x 0x413 +#define STM32F42x_43x 0x419 + +#define REVID_MASK 0xFFFF0000 +#define DEVID_MASK 0xFFF + +#endif + + + +int mcu_version(char* rev, char** revstr) +{ +#ifdef CONFIG_ARCH_CHIP_STM32 + uint32_t abc = getreg32(DBGMCU_IDCODE); + + int32_t chip_version = abc & DEVID_MASK; + enum MCU_REV revid = (abc & REVID_MASK) >> 16; + + switch (chip_version) { + case STM32F40x_41x: + *revstr = "STM32F40x"; + break; + case STM32F42x_43x: + *revstr = "STM32F42x"; + break; + default: + *revstr = "STM32F???"; + break; + } + + switch (revid) { + + case MCU_REV_STM32F4_REV_A: + *rev = 'A'; + break; + case MCU_REV_STM32F4_REV_Z: + *rev = 'Z'; + break; + case MCU_REV_STM32F4_REV_Y: + *rev = 'Y'; + break; + case MCU_REV_STM32F4_REV_1: + *rev = '1'; + break; + case MCU_REV_STM32F4_REV_3: + *rev = '3'; + break; + default: + *rev = '?'; + revid = -1; + break; + } + + return revid; +#else + return -1; +#endif +} diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h new file mode 100644 index 0000000000..1b3d0aba9b --- /dev/null +++ b/src/modules/systemlib/mcu_version.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +#pragma once + +/* magic numbers from reference manual */ +enum MCU_REV { + MCU_REV_STM32F4_REV_A = 0x1000, + MCU_REV_STM32F4_REV_Z = 0x1001, + MCU_REV_STM32F4_REV_Y = 0x1003, + MCU_REV_STM32F4_REV_1 = 0x1007, + MCU_REV_STM32F4_REV_3 = 0x2001 +}; + +/** + * Reports the microcontroller version of the main CPU. + * + * @param rev The silicon revision character + * @param revstr The full chip name string + * @return The silicon revision / version number as integer + */ +__EXPORT int mcu_version(char* rev, char** revstr); diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index bf3428a50b..0d629d6100 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= maxlen) { warnx("line too long"); + fclose(fp); return -1; } @@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen) strcat(buf, line); } + fclose(fp); return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 147903aa03..fe8b7e75a0 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -53,5 +53,7 @@ SRCS = err.c \ otp.c \ board_serial.c \ pwm_limit/pwm_limit.c \ - circuit_breaker.c + circuit_breaker.c \ + mcu_version.c +MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd61..71757e1f42 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -46,6 +46,7 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" +#include "topics/actuator_direct.h" #include "topics/encoders.h" #include "topics/tecs_status.h" @@ -76,6 +77,7 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 0c29101fec..9385ce253c 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -44,3 +44,5 @@ SRCS = uORB.cpp \ objects_common.cpp \ Publication.cpp \ Subscription.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b91a00c1e9..49dfc78349 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/actuator_direct.h" +ORB_DEFINE(actuator_direct, struct actuator_direct_s); + #include "topics/multirotor_motor_limits.h" ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h new file mode 100644 index 0000000000..5f9d0f56dd --- /dev/null +++ b/src/modules/uORB/topics/actuator_direct.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * 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 actuator_direct.h + * + * Actuator direct values. + * + * Values published to this topic are the direct actuator values which + * should be passed to actuators, bypassing mixing + */ + +#ifndef TOPIC_ACTUATOR_DIRECT_H +#define TOPIC_ACTUATOR_DIRECT_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATORS_DIRECT 16 + +/** + * @addtogroup topics + * @{ + */ + +struct actuator_direct_s { + uint64_t timestamp; /**< timestamp in us since system boot */ + float values[NUM_ACTUATORS_DIRECT]; /**< actuator values, from -1 to 1 */ + unsigned nvalues; /**< number of valid values */ +}; + +/** + * @} + */ + +/* actuator direct ORB */ +ORB_DECLARE(actuator_direct); + +#endif // TOPIC_ACTUATOR_DIRECT_H diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 15b55648d0..13138ebc9a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -47,7 +47,7 @@ * Switch position */ typedef enum { - SWITCH_POS_NONE = 0, /**< switch is not mapped */ + SWITCH_POS_NONE = 0, /**< switch is not mapped */ SWITCH_POS_ON, /**< switch activated (value = 1) */ SWITCH_POS_MIDDLE, /**< middle position (value = 0) */ SWITCH_POS_OFF /**< switch not activated (value = -1) */ @@ -93,13 +93,13 @@ struct manual_control_setpoint_s { float aux4; /**< default function: camera roll */ float aux5; /**< default function: payload drop */ - switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ - switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ - switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ - switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ - switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ - switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ -}; /**< manual control inputs */ + switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ + switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ + switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ + switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ + switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */ +}; /** * @} diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h index 0196ae86b0..d3dc46ee0b 100644 --- a/src/modules/uORB/topics/optical_flow.h +++ b/src/modules/uORB/topics/optical_flow.h @@ -55,16 +55,22 @@ */ struct optical_flow_s { - uint64_t timestamp; /**< in microseconds since system start */ - - uint64_t flow_timestamp; /**< timestamp from flow sensor */ - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ + uint64_t timestamp; /**< in microseconds since system start */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ + float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ + float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ + float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ + float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ + float ground_distance_m; /**< Altitude / distance to ground in meters */ + uint32_t integration_timespan; /** + +#define MOTOR_BIT(x) (1<<(x)) + UavcanEscController::UavcanEscController(uavcan::INode &node) : _node(node), _uavcan_pub_raw_cmd(node), @@ -73,7 +76,9 @@ int UavcanEscController::init() void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) { - if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) { + if ((outputs == nullptr) || + (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) || + (num_outputs > CONNECTED_ESC_MAX)) { perf_count(_perfcnt_invalid_input); return; } @@ -95,14 +100,18 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - if (_armed) { - for (unsigned i = 0; i < num_outputs; i++) { - + for (unsigned i = 0; i < num_outputs; i++) { + if (_armed_mask & MOTOR_BIT(i)) { float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max; - if (scaled < 1.0F) { - scaled = 1.0F; // Since we're armed, we don't want to stop it completely - } - + // trim negative values back to 0. Previously + // we set this to 0.1, which meant motors kept + // spinning when armed, but that should be a + // policy decision for a specific vehicle + // type, as it is not appropriate for all + // types of vehicles (eg. fixed wing). + if (scaled < 0.0F) { + scaled = 0.0F; + } if (scaled > cmd_max) { scaled = cmd_max; perf_count(_perfcnt_scaling_error); @@ -111,6 +120,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) msg.cmd.push_back(static_cast(scaled)); _esc_status.esc[i].esc_setpoint_raw = abs(static_cast(scaled)); + } else { + msg.cmd.push_back(static_cast(0)); } } @@ -121,9 +132,22 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs) (void)_uavcan_pub_raw_cmd.broadcast(msg); } -void UavcanEscController::arm_esc(bool arm) +void UavcanEscController::arm_all_escs(bool arm) { - _armed = arm; + if (arm) { + _armed_mask = -1; + } else { + _armed_mask = 0; + } +} + +void UavcanEscController::arm_single_esc(int num, bool arm) +{ + if (arm) { + _armed_mask = MOTOR_BIT(num); + } else { + _armed_mask = 0; + } } void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure &msg) diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp index f4a3877e62..12c0355422 100644 --- a/src/modules/uavcan/actuators/esc.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -61,7 +61,8 @@ public: void update_outputs(float *outputs, unsigned num_outputs); - void arm_esc(bool arm); + void arm_all_escs(bool arm); + void arm_single_esc(int num, bool arm); private: /** @@ -98,6 +99,11 @@ private: uavcan::Subscriber _uavcan_sub_status; uavcan::TimerEventForwarder _orb_timer; + /* + * ESC states + */ + uint32_t _armed_mask = 0; + /* * Perf counters */ diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 80c5e3828e..8741ae20dd 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure< { auto report = ::baro_report(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - + report.timestamp = msg.getMonotonicTimestamp().toUSec(); report.temperature = msg.static_temperature; report.pressure = msg.static_pressure / 100.0F; // Convert to millibar diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 24afe6aaf4..a375db37f8 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure - #include #include diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 8e6a9a22f0..35ebee5426 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -37,6 +37,8 @@ #include "mag.hpp" +#include + static const orb_id_t MAG_TOPICS[3] = { ORB_ID(sensor_mag0), ORB_ID(sensor_mag1), @@ -49,6 +51,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), _sub_mag(node) { + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; + _scale.x_scale = 1.0F; _scale.y_scale = 1.0F; _scale.z_scale = 1.0F; @@ -69,9 +73,36 @@ int UavcanMagnetometerBridge::init() return 0; } +ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen) +{ + static uint64_t last_read = 0; + struct mag_report *mag_buf = reinterpret_cast(buffer); + + /* buffer must be large enough */ + unsigned count = buflen / sizeof(struct mag_report); + if (count < 1) { + return -ENOSPC; + } + + if (last_read < _report.timestamp) { + /* copy report */ + lock(); + *mag_buf = _report; + last_read = _report.timestamp; + unlock(); + return sizeof(struct mag_report); + } else { + /* no new data available, warn caller */ + return -EAGAIN; + } +} + int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCSQUEUEDEPTH: { + return OK; // Pretend that this stuff is supported to keep APM happy + } case MAGIOCSSCALE: { std::memcpy(&_scale, reinterpret_cast(arg), sizeof(_scale)); return 0; @@ -84,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar return 0; // Nothing to do } case MAGIOCGEXTERNAL: { - return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + return 1; // declare it external rise it's priority and to allow for correct orientation compensation } case MAGIOCSSAMPLERATE: { return 0; // Pretend that this stuff is supported to keep the sensor app happy @@ -106,18 +137,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure &msg) { - auto report = ::mag_report(); + lock(); + _report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + _report.timestamp = msg.getMonotonicTimestamp().toUSec(); - report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + _report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + _report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + _report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + unlock(); - report.timestamp = msg.getUtcTimestamp().toUSec(); - if (report.timestamp == 0) { - report.timestamp = msg.getMonotonicTimestamp().toUSec(); - } - - report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; - report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; - report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; - - publish(msg.getSrcNodeID().get(), &report); + publish(msg.getSrcNodeID().get(), &_report); } diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 6d413a8f71..74077d883e 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -54,6 +54,7 @@ public: int init() override; private: + ssize_t read(struct file *filp, char *buffer, size_t buflen); int ioctl(struct file *filp, int cmd, unsigned long arg) override; void mag_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -65,4 +66,5 @@ private: uavcan::Subscriber _sub_mag; mag_scale _scale = {}; + mag_report _report = {}; }; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 9608ce6804..0999938fc3 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) return; } + // update device id as we now know our device node_id + _device_id.devid_s.address = static_cast(node_id); + // Ask the CDev helper which class instance we can take const int class_instance = register_class_devname(_class_devname); if (class_instance < 0 || class_instance >= int(_max_channels)) { diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 1316f7ecc5..e319605370 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -112,6 +112,8 @@ protected: _channels(new Channel[MaxChannels]) { memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; + _device_id.devid_s.bus = 0; } /** diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index a8485ee44e..60901e0c75 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -46,6 +46,8 @@ #include #include +#include + #include #include @@ -269,6 +271,24 @@ void UavcanNode::node_spin_once() } } +/* + add a fd to the list of polled events. This assumes you want + POLLIN for now. + */ +int UavcanNode::add_poll_fd(int fd) +{ + int ret = _poll_fds_num; + if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) { + errx(1, "uavcan: too many poll fds, exiting"); + } + _poll_fds[_poll_fds_num] = ::pollfd(); + _poll_fds[_poll_fds_num].fd = fd; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num += 1; + return ret; +} + + int UavcanNode::run() { (void)pthread_mutex_lock(&_node_mutex); @@ -279,9 +299,10 @@ int UavcanNode::run() _output_count = 2; _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + _test_motor_sub = orb_subscribe(ORB_ID(test_motor)); + _actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct)); - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); + memset(&_outputs, 0, sizeof(_outputs)); const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0); if (busevent_fd < 0) @@ -303,11 +324,15 @@ int UavcanNode::run() * the value returned from poll() to detect whether actuator control has timed out or not. * Instead, all ORB events need to be checked individually (see below). */ - _poll_fds_num = 0; - _poll_fds[_poll_fds_num] = ::pollfd(); - _poll_fds[_poll_fds_num].fd = busevent_fd; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num += 1; + add_poll_fd(busevent_fd); + + /* + * setup poll to look for actuator direct input if we are + * subscribed to the topic + */ + if (_actuator_direct_sub != -1) { + _actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub); + } while (!_task_should_exit) { // update actuator controls subscriptions if needed @@ -325,6 +350,8 @@ int UavcanNode::run() node_spin_once(); // Non-blocking + bool new_output = false; + // this would be bad... if (poll_ret < 0) { log("poll error %d", errno); @@ -332,67 +359,102 @@ int UavcanNode::run() } else { // get controls for required topics bool controls_updated = false; - unsigned poll_id = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (_control_subs[i] > 0) { - if (_poll_fds[poll_id].revents & POLLIN) { + if (_poll_fds[_poll_ids[i]].revents & POLLIN) { controls_updated = true; orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - poll_id++; } } + /* + see if we have any direct actuator updates + */ + if (_actuator_direct_sub != -1 && + (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) && + orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK && + !_test_in_progress) { + if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) { + _actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS; + } + memcpy(&_outputs.output[0], &_actuator_direct.values[0], + _actuator_direct.nvalues*sizeof(float)); + _outputs.noutputs = _actuator_direct.nvalues; + new_output = true; + } + // can we mix? - if (controls_updated && (_mixers != nullptr)) { + if (_test_in_progress) { + memset(&_outputs, 0, sizeof(_outputs)); + if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) { + _outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f; + _outputs.noutputs = _test_motor.motor_number+1; + } + new_output = true; + } else if (controls_updated && (_mixers != nullptr)) { // XXX one output group has 8 outputs max, // but this driver could well serve multiple groups. unsigned num_outputs_max = 8; // Do mixing - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max); - outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max); - // iterate actuators - for (unsigned i = 0; i < outputs.noutputs; i++) { - // last resort: catch NaN, INF and out-of-band errors - if (!isfinite(outputs.output[i])) { - /* - * 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; - } - - // limit outputs to valid range - - // never go below min - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - } - - // never go below max - if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - } - } - - // Output to the bus - _esc_controller.update_outputs(outputs.output, outputs.noutputs); + new_output = true; } } - // Check arming state + if (new_output) { + // iterate actuators, checking for valid values + for (uint8_t i = 0; i < _outputs.noutputs; i++) { + // last resort: catch NaN, INF and out-of-band errors + if (!isfinite(_outputs.output[i])) { + /* + * 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; + } + + // never go below min + if (_outputs.output[i] < -1.0f) { + _outputs.output[i] = -1.0f; + } + + // never go above max + if (_outputs.output[i] > 1.0f) { + _outputs.output[i] = 1.0f; + } + } + // Output to the bus + _outputs.timestamp = hrt_absolute_time(); + _esc_controller.update_outputs(_outputs.output, _outputs.noutputs); + } + + + // Check motor test state bool updated = false; + orb_check(_test_motor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor); + + // Update the test status and check that we're not locked down + _test_in_progress = (_test_motor.value > 0); + _esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress); + } + + // Check arming state orb_check(_armed_sub, &updated); if (updated) { orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - // Update the armed status and check that we're not locked down - bool set_armed = _armed.armed && !_armed.lockdown; + // Update the armed status and check that we're not locked down and motor + // test is not running + bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress; arm_actuators(set_armed); } @@ -429,7 +491,7 @@ int UavcanNode::arm_actuators(bool arm) { _is_armed = arm; - _esc_controller.arm_esc(arm); + _esc_controller.arm_all_escs(arm); return OK; } @@ -440,7 +502,6 @@ UavcanNode::subscribe() uint32_t sub_groups = _groups_required & ~_groups_subscribed; uint32_t unsub_groups = _groups_subscribed & ~_groups_required; // the first fd used by CAN - _poll_fds_num = 1; for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { if (sub_groups & (1 << i)) { warnx("subscribe to actuator_controls_%d", i); @@ -453,9 +514,7 @@ UavcanNode::subscribe() } if (_control_subs[i] > 0) { - _poll_fds[_poll_fds_num].fd = _control_subs[i]; - _poll_fds[_poll_fds_num].events = POLLIN; - _poll_fds_num++; + _poll_ids[i] = add_poll_fd(_control_subs[i]); } } } @@ -553,6 +612,36 @@ UavcanNode::print_info() (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + if (_outputs.noutputs != 0) { + printf("ESC output: "); + + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d ", (int)(_outputs.output[i]*1000)); + } + printf("\n"); + + // ESC status + int esc_sub = orb_subscribe(ORB_ID(esc_status)); + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), esc_sub, &esc); + + printf("ESC Status:\n"); + printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n"); + for (uint8_t i=0; i<_outputs.noutputs; i++) { + printf("%d\t", esc.esc[i].esc_address); + printf("%3.2f\t", (double)esc.esc[i].esc_voltage); + printf("%3.2f\t", (double)esc.esc[i].esc_current); + printf("%3.2f\t", (double)esc.esc[i].esc_temperature); + printf("%3.2f\t", (double)esc.esc[i].esc_setpoint); + printf("%d\t", esc.esc[i].esc_rpm); + printf("%d", esc.esc[i].esc_errorcount); + printf("\n"); + } + + orb_unsubscribe(esc_sub); + } + // Sensor bridges auto br = _sensor_bridges.getHead(); while (br != nullptr) { @@ -571,7 +660,7 @@ UavcanNode::print_info() static void print_usage() { warnx("usage: \n" - "\tuavcan {start|status|stop}"); + "\tuavcan {start|status|stop|arm|disarm}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); @@ -618,6 +707,16 @@ int uavcan_main(int argc, char *argv[]) ::exit(0); } + if (!std::strcmp(argv[1], "arm")) { + inst->arm_actuators(true); + ::exit(0); + } + + if (!std::strcmp(argv[1], "disarm")) { + inst->arm_actuators(false); + ::exit(0); + } + if (!std::strcmp(argv[1], "stop")) { delete inst; ::exit(0); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index be7db97411..98f2e5ad4b 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" @@ -56,6 +58,9 @@ #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" +// we add two to allow for actuator_direct and busevent +#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2) + /** * A UAVCAN node. */ @@ -96,6 +101,8 @@ private: int init(uavcan::NodeID node_id); void node_spin_once(); int run(); + int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[] + int _task = -1; ///< handle to the OS task bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver @@ -103,6 +110,10 @@ private: actuator_armed_s _armed; ///< the arming request of the system bool _is_armed = false; ///< the arming status of the actuators on the bus + int _test_motor_sub = -1; ///< uORB subscription of the test_motor status + test_motor_s _test_motor; + bool _test_in_progress = false; + unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer @@ -120,6 +131,15 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {}; - pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent + pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {}; unsigned _poll_fds_num = 0; + + int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic + uint8_t _actuator_direct_poll_fd_num; + actuator_direct_s _actuator_direct; + + actuator_outputs_s _outputs; + + // index into _poll_fds for each _control_subs handle + uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN]; }; diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk index eb36d2ded8..261428ef94 100644 --- a/src/systemcmds/motor_test/module.mk +++ b/src/systemcmds/motor_test/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = motor_test SRCS = motor_test.c MODULE_STACKSIZE = 4096 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 079f99674a..77dc2f0d50 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -54,32 +54,35 @@ #include "systemlib/err.h" -__EXPORT int motor_test_main(int argc, char *argv[]); +__EXPORT int motor_test_main(int argc, char *argv[]); + static void motor_test(unsigned channel, float value); static void usage(const char *reason); +static orb_advert_t _test_motor_pub; + void motor_test(unsigned channel, float value) { - orb_advert_t _test_motor_pub; struct test_motor_s _test_motor; _test_motor.motor_number = channel; _test_motor.timestamp = hrt_absolute_time(); _test_motor.value = value; - if (_test_motor_pub > 0) { - /* publish armed state */ - orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); - } else { - /* advertise and publish */ - _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); - } + if (_test_motor_pub > 0) { + /* publish test state */ + orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor); + } else { + /* advertise and publish */ + _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor); + } } static void usage(const char *reason) { - if (reason != NULL) + if (reason != NULL) { warnx("%s", reason); + } errx(1, "usage:\n" @@ -90,8 +93,9 @@ static void usage(const char *reason) int motor_test_main(int argc, char *argv[]) { - unsigned long channel, lval; - float value; + unsigned long channel = 0; + unsigned long lval; + float value = 0.0f; int ch; if (argc != 5) { @@ -102,18 +106,18 @@ int motor_test_main(int argc, char *argv[]) switch (ch) { case 'm': - /* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */ + /* Read in motor number */ channel = strtoul(optarg, NULL, 0); break; case 'p': - /* Read in custom low value */ + /* Read in power value */ lval = strtoul(optarg, NULL, 0); if (lval > 100) usage("value invalid"); - value = (float)lval/100.f; + value = ((float)lval)/100.f; break; default: usage(NULL); @@ -122,7 +126,7 @@ int motor_test_main(int argc, char *argv[]) motor_test(channel, value); - printf("motor %d set to %.2f\n", channel, value); + printf("motor %d set to %.2f\n", channel, (double)value); exit(0); } diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index f06c49552e..edeb5c6248 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -51,6 +51,8 @@ #include #include +#include + __EXPORT int nshterm_main(int argc, char *argv[]); int @@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[]) } unsigned retries = 0; int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + /* we assume the system does not provide arming status feedback */ + bool armed_updated = false; + + /* try the first 30 seconds or if arming system is ready */ + while ((retries < 300) || armed_updated) { + + /* abort if an arming topic is published and system is armed */ + bool updated = false; + if (orb_check(armed_fd, &updated)) { + /* the system is now providing arming status feedback. + * instead of timing out, we resort to abort bringing + * up the terminal. + */ + armed_updated = true; + orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); + + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } - /* try the first 30 seconds */ - while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 478c2a772f..eeba89fa85 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -74,34 +74,34 @@ usage(const char *reason) "usage:\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n" "\n" - " arm Arm output\n" - " disarm Disarm output\n" + "arm\t\t\t\tArm output\n" + "disarm\t\t\t\tDisarm output\n" "\n" - " rate ... Configure PWM rates\n" - " [-g ] Channel group that should update at the alternate rate\n" - " [-m ] Directly supply channel mask\n" - " [-a] Configure all outputs\n" - " -r PWM rate (50 to 400 Hz)\n" + "rate ...\t\t\tConfigure PWM rates\n" + "\t[-g ]\t(e.g. 0,1,2)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-r \t\tPWM 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 ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "failsafe ...\t\t\tFailsafe PWM\n" + "disarmed ...\t\t\tDisarmed PWM\n" + "min ...\t\t\t\tMinimum PWM\n" + "max ...\t\t\t\tMaximum PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " test ... Directly set PWM values\n" - " [-c ] Supply channels (e.g. 1234)\n" - " [-m ] Directly supply channel mask (e.g. 0xF)\n" - " [-a] Configure all outputs\n" - " -p PWM value\n" + "test ...\t\t\tDirectly set PWM\n" + "\t[-c ]\t\t(e.g. 1234)\n" + "\t[-m ]\t(e.g. 0xF)\n" + "\t[-a]\t\t\tConfigure all outputs\n" + "\t-p \t\tPWM value\n" "\n" - " info Print information about the PWM device\n" + "info\t\t\t\tPrint information\n" "\n" - " -v Print verbose information\n" - " -d PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + "\t-v\t\t\tVerbose\n" + "\t-d \t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n" ); } @@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[]) unsigned single_ch = 0; unsigned pwm_value = 0; - if (argc < 1) + if (argc < 2) usage(NULL); while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) { @@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[]) /* Read in mask directly */ set_mask = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad set_mask value"); + usage("BAD set_mask VAL"); break; case 'a': @@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[]) case 'p': pwm_value = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad PWM value provided"); + usage("BAD PWM VAL"); break; case 'r': alt_rate = strtoul(optarg, &ep, 0); if (*ep != '\0') - usage("bad alternative rate provided"); + usage("BAD rate VAL"); break; default: break; @@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[]) } if (print_verbose && set_mask > 0) { - warnx("Chose channels: "); + warnx("Channels: "); printf(" "); for (unsigned i = 0; i +#include +#include +#include +#include +#include +#include +#include + +__EXPORT int reflect_main(int argc, char *argv[]); + +// memory corruption checking +#define MAX_BLOCKS 1000 +static uint32_t nblocks; +struct block { + uint32_t v[256]; +}; +static struct block *blocks[MAX_BLOCKS]; + +#define VALUE(i) ((i*7) ^ 0xDEADBEEF) + +static void allocate_blocks(void) +{ + while (nblocks < MAX_BLOCKS) { + blocks[nblocks] = calloc(1, sizeof(struct block)); + if (blocks[nblocks] == NULL) { + break; + } + for (uint32_t i=0; iv)/sizeof(uint32_t); i++) { + blocks[nblocks]->v[i] = VALUE(i); + } + nblocks++; + } + printf("Allocated %u blocks\n", nblocks); +} + +static void check_blocks(void) +{ + for (uint32_t n=0; nv)/sizeof(uint32_t); i++) { + assert(blocks[n]->v[i] == VALUE(i)); + } + } +} + +int +reflect_main(int argc, char *argv[]) +{ + uint32_t total = 0; + printf("Starting reflector\n"); + + allocate_blocks(); + + while (true) { + char buf[128]; + ssize_t n = read(0, buf, sizeof(buf)); + if (n < 0) { + break; + } + if (n > 0) { + write(1, buf, n); + } + total += n; + if (total > 1024000) { + check_blocks(); + total = 0; + } + } + return OK; +} diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 9ae080ee21..2ead3e6323 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -44,14 +44,16 @@ #include #include #include +#include -// string constants for version commands +/* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; static const char sz_ver_git_str[] = "git"; static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; +static const char mcu_ver_str[] = "mcu"; static void usage(const char *reason) { @@ -59,55 +61,87 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n"); } __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) { - int ret = 1; //defaults to an error + /* defaults to an error */ + int ret = 1; - // first check if there are at least 2 params + /* first check if there are at least 2 params */ if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { - printf("%s\n", HW_ARCH); - ret = 0; - } else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { + if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { - // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + /* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */ ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { printf("ver hwcmp match: %s\n", HW_ARCH); } + return ret; } else { errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } + } - } else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("%s\n", FW_GIT); - ret = 0; + /* check if we want to show all */ + bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str)); - } else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { - printf("%s %s\n", __DATE__, __TIME__); - ret = 0; - - } else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { - printf("%s\n", __VERSION__); - ret = 0; - - } else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { + if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { printf("HW arch: %s\n", HW_ARCH); - printf("Build datetime: %s %s\n", __DATE__, __TIME__); - printf("FW git-hash: %s\n", FW_GIT); - printf("GCC toolchain: %s\n", __VERSION__); ret = 0; - } else { + } + + if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { + printf("FW git-hash: %s\n", FW_GIT); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + printf("Build datetime: %s %s\n", __DATE__, __TIME__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { + printf("Toolchain: %s\n", __VERSION__); + ret = 0; + + } + + if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) { + + char rev; + char* revstr; + + int chip_version = mcu_version(&rev, &revstr); + + if (chip_version < 0) { + printf("UNKNOWN MCU\n"); + + } else { + printf("MCU: %s, rev. %c\n", revstr, rev); + + if (chip_version < MCU_REV_STM32F4_REV_3) { + printf("\nWARNING WARNING WARNING!\n" + "Revision %c has a silicon errata\n" + "This device can only utilize a maximum of 1MB flash safely!\n" + "http://px4.io/help/errata\n\n", rev); + } + } + + ret = 0; + } + + if (ret == 1) { errx(1, "unknown command.\n"); }