diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py new file mode 100644 index 0000000000..b0864a2293 --- /dev/null +++ b/Debug/Nuttx.py @@ -0,0 +1,298 @@ +# GDB/Python functions for dealing with NuttX + +import gdb, gdb.types + +class NX_task(object): + """Reference to a NuttX task and methods for introspecting it""" + + def __init__(self, tcb_ptr): + self._tcb = tcb_ptr.dereference() + self._group = self._tcb['group'].dereference() + self.pid = tcb_ptr['pid'] + + @classmethod + def for_tcb(cls, tcb): + """return a task with the given TCB pointer""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + pidhash_entry = pidhash_value[i] + if pidhash_entry['tcb'] == tcb: + return cls(pidhash_entry['tcb']) + return None + + @classmethod + def for_pid(cls, pid): + """return a task for the given PID""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + pidhash_entry = pidhash_value[i] + if pidhash_entry['pid'] == pid: + return cls(pidhash_entry['tcb']) + return None + + @staticmethod + def pids(): + """return a list of all PIDs""" + pidhash_sym = gdb.lookup_global_symbol('g_pidhash') + pidhash_value = pidhash_sym.value() + pidhash_type = pidhash_sym.type + result = [] + for i in range(pidhash_type.range()[0],pidhash_type.range()[1]): + entry = pidhash_value[i] + pid = int(entry['pid']) + if pid is not -1: + result.append(pid) + return result + + @staticmethod + def tasks(): + """return a list of all tasks""" + tasks = [] + for pid in NX_task.pids(): + tasks.append(NX_task.for_pid(pid)) + return tasks + + def _state_is(self, state): + """tests the current state of the task against the passed-in state name""" + statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) + if self._tcb['task_state'] == statenames[state]: + return True + return False + + @property + def stack_used(self): + """calculate the stack used by the thread""" + if 'stack_used' not in self.__dict__: + stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer()) + if stack_base == 0: + self.__dict__['stack_used'] = 0 + else: + stack_limit = self._tcb['adj_stack_size'] + for offset in range(0, stack_limit): + if stack_base[offset] != 0xff: + break + self.__dict__['stack_used'] = stack_limit - offset + return self.__dict__['stack_used'] + + @property + def name(self): + """return the task's name""" + return self._tcb['name'].string() + + @property + def state(self): + """return the name of the task's current state""" + statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e')) + for name,value in statenames.iteritems(): + if value == self._tcb['task_state']: + return name + return 'UNKNOWN' + + @property + def waiting_for(self): + """return a description of what the task is waiting for, if it is waiting""" + if self._state_is('TSTATE_WAIT_SEM'): + waitsem = self._tcb['waitsem'].dereference() + waitsem_holder = waitsem['holder'] + holder = NX_task.for_tcb(waitsem_holder['htcb']) + if holder is not None: + return '{}({})'.format(waitsem.address, holder.name) + else: + return '{}()'.format(waitsem.address) + if self._state_is('TSTATE_WAIT_SIG'): + return 'signal' + return None + + @property + def is_waiting(self): + """tests whether the task is waiting for something""" + if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'): + return True + + @property + def is_runnable(self): + """tests whether the task is runnable""" + if (self._state_is('TSTATE_TASK_PENDING') or + self._state_is('TSTATE_TASK_READYTORUN') or + self._state_is('TSTATE_TASK_RUNNING')): + return True + return False + + @property + def file_descriptors(self): + """return a dictionary of file descriptors and inode pointers""" + filelist = self._group['tg_filelist'] + filearray = filelist['fl_files'] + result = dict() + for i in range(filearray.type.range()[0],filearray.type.range()[1]): + inode = long(filearray[i]['f_inode']) + if inode != 0: + result[i] = inode + return result + + @property + def registers(self): + if 'registers' not in self.__dict__: + registers = dict() + if self._state_is('TSTATE_TASK_RUNNING'): + # XXX need to fix this to handle interrupt context + registers['R0'] = long(gdb.parse_and_eval('$r0')) + registers['R1'] = long(gdb.parse_and_eval('$r1')) + registers['R2'] = long(gdb.parse_and_eval('$r2')) + registers['R3'] = long(gdb.parse_and_eval('$r3')) + registers['R4'] = long(gdb.parse_and_eval('$r4')) + registers['R5'] = long(gdb.parse_and_eval('$r5')) + registers['R6'] = long(gdb.parse_and_eval('$r6')) + registers['R7'] = long(gdb.parse_and_eval('$r7')) + registers['R8'] = long(gdb.parse_and_eval('$r8')) + registers['R9'] = long(gdb.parse_and_eval('$r9')) + registers['R10'] = long(gdb.parse_and_eval('$r10')) + registers['R11'] = long(gdb.parse_and_eval('$r11')) + registers['R12'] = long(gdb.parse_and_eval('$r12')) + registers['R13'] = long(gdb.parse_and_eval('$r13')) + registers['SP'] = long(gdb.parse_and_eval('$sp')) + registers['R14'] = long(gdb.parse_and_eval('$r14')) + registers['LR'] = long(gdb.parse_and_eval('$lr')) + registers['R15'] = long(gdb.parse_and_eval('$r15')) + registers['PC'] = long(gdb.parse_and_eval('$pc')) + registers['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + # this would only be valid if we were in an interrupt + registers['EXC_RETURN'] = 0 + # we should be able to get this... + registers['PRIMASK'] = 0 + else: + context = self._tcb['xcp'] + regs = context['regs'] + registers['R0'] = long(regs[27]) + registers['R1'] = long(regs[28]) + registers['R2'] = long(regs[29]) + registers['R3'] = long(regs[30]) + registers['R4'] = long(regs[2]) + registers['R5'] = long(regs[3]) + registers['R6'] = long(regs[4]) + registers['R7'] = long(regs[5]) + registers['R8'] = long(regs[6]) + registers['R9'] = long(regs[7]) + registers['R10'] = long(regs[8]) + registers['R11'] = long(regs[9]) + registers['R12'] = long(regs[31]) + registers['R13'] = long(regs[0]) + registers['SP'] = long(regs[0]) + registers['R14'] = long(regs[32]) + registers['LR'] = long(regs[32]) + registers['R15'] = long(regs[33]) + registers['PC'] = long(regs[33]) + registers['XPSR'] = long(regs[34]) + registers['EXC_RETURN'] = long(regs[10]) + registers['PRIMASK'] = long(regs[1]) + self.__dict__['registers'] = registers + return self.__dict__['registers'] + + def __repr__(self): + return "".format(self.pid) + + def __str__(self): + return "{}:{}".format(self.pid, self.name) + + def __format__(self, format_spec): + return format_spec.format( + pid = self.pid, + name = self.name, + state = self.state, + waiting_for = self.waiting_for, + stack_used = self.stack_used, + stack_limit = self._tcb['adj_stack_size'], + file_descriptors = self.file_descriptors, + registers = self.registers + ) + +class NX_show_task (gdb.Command): + """(NuttX) prints information about a task""" + + def __init__(self): + super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER) + + def invoke(self, arg, from_tty): + t = NX_task.for_pid(int(arg)) + if t is not None: + my_fmt = 'PID:{pid} name:{name} state:{state}\n' + my_fmt += ' stack used {stack_used} of {stack_limit}\n' + if t.is_waiting: + my_fmt += ' waiting for {waiting_for}\n' + my_fmt += ' open files: {file_descriptors}\n' + my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n' + my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n' + my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' + my_fmt += ' R12 {registers[PC]:#010x}\n' + my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' + print format(t, my_fmt) + +class NX_show_tasks (gdb.Command): + """(NuttX) prints a list of tasks""" + + def __init__(self): + super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER) + + def invoke(self, args, from_tty): + tasks = NX_task.tasks() + for t in tasks: + print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}') + +NX_show_task() +NX_show_tasks() + +class NX_show_heap (gdb.Command): + """(NuttX) prints the heap""" + + def __init__(self): + super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER) + if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8: + self._allocflag = 0x80000000 + self._allocnodesize = 8 + else: + self._allocflag = 0x8000 + self._allocnodesize = 4 + + def _node_allocated(self, allocnode): + if allocnode['preceding'] & self._allocflag: + return True + return False + + def _node_size(self, allocnode): + return allocnode['size'] & ~self._allocflag + + def _print_allocations(self, region_start, region_end): + if region_start >= region_end: + print 'heap region {} corrupt'.format(hex(region_start)) + return + nodecount = region_end - region_start + print 'heap {} - {}'.format(region_start, region_end) + cursor = 1 + while cursor < nodecount: + allocnode = region_start[cursor] + if self._node_allocated(allocnode): + state = '' + else: + state = '(free)' + print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state) + cursor += self._node_size(allocnode) / self._allocnodesize + + def invoke(self, args, from_tty): + heap = gdb.lookup_global_symbol('g_mmheap').value() + nregions = heap['mm_nregions'] + region_starts = heap['mm_heapstart'] + region_ends = heap['mm_heapend'] + print "{} heap(s)".format(nregions) + # walk the heaps + for i in range(0, nregions): + self._print_allocations(region_starts[i], region_ends[i]) + +NX_show_heap() + + + + diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 861d59b4b1..a7a6725c65 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y # CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RXDMA is not set # CONFIG_UART5_RS485 is not set -CONFIG_UART5_RXDMA=n +CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set CONFIG_USART6_RXDMA=y # CONFIG_USART7_RXDMA is not set diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0f90db858a..b1712e85eb 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -186,8 +186,10 @@ public: int set_idle_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO - */ + * Print IO status. + * + * Print all relevant IO status information + */ void print_status(); /** @@ -235,16 +237,16 @@ private: uint16_t _alarms; ///armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } else { + /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b2f6b33e3c..0d5a537eac 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,14 +211,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool reset_sp_alt = true; - bool reset_sp_pos = true; + bool global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_integral = true; + hrt_abstime t_prev = 0; /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float home_alt = 0.0f; - hrt_abstime home_alt_t = 0; + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -242,11 +251,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; - bool global_pos_sp_updated = false; while (!thread_should_exit) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz */ if (++paramcheck_counter >= 50) { paramcheck_counter = 0; @@ -260,11 +266,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); /* use integral_limit_out = tilt_max / 2 */ float i_limit; + if (params.xy_vel_i == 0.0) { i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { i_limit = 1.0f; // not used really } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -273,9 +282,20 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* only check global position setpoint updates but not read */ - if (!global_pos_sp_updated) { - orb_check(global_pos_sp_sub, &global_pos_sp_updated); + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + global_pos_sp_reproject = true; } hrt_abstime t = hrt_absolute_time(); @@ -288,6 +308,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) dt = 0.0f; } + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + t_prev = t; if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { @@ -296,77 +326,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (control_mode.flag_control_manual_enabled) { - /* manual mode, reset setpoints if needed */ - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside - mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - } - - if (control_mode.flag_control_position_enabled && reset_sp_pos) { - reset_sp_pos = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - } - } else { - /* non-manual mode, project global setpoints to local frame */ - //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.ref_timestamp != local_ref_timestamp) { - local_ref_timestamp = local_pos.ref_timestamp; - /* init local projection using local position home */ - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); - mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_updated) { - global_pos_sp_updated = false; - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - } - } - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.ref_timestamp != home_alt_t) { - if (home_alt_t != 0) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - ref_alt; } - home_alt_t = local_pos.ref_timestamp; - home_alt = local_pos.ref_alt; + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint } + /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with manual controls */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { @@ -383,7 +369,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - /* move position setpoint with manual controls */ + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); @@ -410,12 +405,68 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = true; + + } else { + /* non-manual mode, use global setpoint */ + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + } else { + reset_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -426,13 +477,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { - reset_sp_pos = true; + reset_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -444,20 +496,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - bool reset_integral = !control_mode.flag_armed; + if (control_mode.flag_control_climb_rate_enabled) { - if (reset_integral) { - thrust_pid_set_integral(&z_vel_pid, params.thr_min); + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); } + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; } + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ - if (reset_integral) { + if (reset_int_xy) { + reset_int_xy = false; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); @@ -471,11 +547,15 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (tilt > params.tilt_max) { tilt = params.tilt_max; } + /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; } att_sp.timestamp = hrt_absolute_time(); @@ -483,14 +563,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else { - reset_sp_alt = true; - reset_sp_pos = true; + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* run at approximately 50 Hz */ usleep(20000); - } warnx("stopped"); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1094fd23b7..0b09c9ea77 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +34,7 @@ /* * @file multirotor_pos_control_params.c - * + * * Parameters for multirotor_pos_control */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 14a3de2e59..3ec85a3641 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +33,9 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include 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 81f938719c..c0cfac880e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -508,6 +508,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -554,6 +555,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* publish global position */ global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); @@ -561,21 +563,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); } + if (local_pos.z_valid) { global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index dd98f65a9c..d99892fe2e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -259,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 67aeac3728..4f4db5dbcb 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,7 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; @@ -73,11 +73,9 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // XXX shouldn't be necessairy - //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */