forked from Archive/PX4-Autopilot
Merge branch 'fmuv2_bringup_new_state_machine_drton' of https://github.com/cvg/Firmware_Private into fmuv2_bringup_new_state_machine_drton
This commit is contained in:
Simon Wilks
commit
ecc5019a35
|
|
@ -0,0 +1,298 @@
|
|||
# GDB/Python functions for dealing with NuttX
|
||||
|
||||
import gdb, gdb.types
|
||||
|
||||
class NX_task(object):
|
||||
"""Reference to a NuttX task and methods for introspecting it"""
|
||||
|
||||
def __init__(self, tcb_ptr):
|
||||
self._tcb = tcb_ptr.dereference()
|
||||
self._group = self._tcb['group'].dereference()
|
||||
self.pid = tcb_ptr['pid']
|
||||
|
||||
@classmethod
|
||||
def for_tcb(cls, tcb):
|
||||
"""return a task with the given TCB pointer"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
pidhash_entry = pidhash_value[i]
|
||||
if pidhash_entry['tcb'] == tcb:
|
||||
return cls(pidhash_entry['tcb'])
|
||||
return None
|
||||
|
||||
@classmethod
|
||||
def for_pid(cls, pid):
|
||||
"""return a task for the given PID"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
pidhash_entry = pidhash_value[i]
|
||||
if pidhash_entry['pid'] == pid:
|
||||
return cls(pidhash_entry['tcb'])
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def pids():
|
||||
"""return a list of all PIDs"""
|
||||
pidhash_sym = gdb.lookup_global_symbol('g_pidhash')
|
||||
pidhash_value = pidhash_sym.value()
|
||||
pidhash_type = pidhash_sym.type
|
||||
result = []
|
||||
for i in range(pidhash_type.range()[0],pidhash_type.range()[1]):
|
||||
entry = pidhash_value[i]
|
||||
pid = int(entry['pid'])
|
||||
if pid is not -1:
|
||||
result.append(pid)
|
||||
return result
|
||||
|
||||
@staticmethod
|
||||
def tasks():
|
||||
"""return a list of all tasks"""
|
||||
tasks = []
|
||||
for pid in NX_task.pids():
|
||||
tasks.append(NX_task.for_pid(pid))
|
||||
return tasks
|
||||
|
||||
def _state_is(self, state):
|
||||
"""tests the current state of the task against the passed-in state name"""
|
||||
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
|
||||
if self._tcb['task_state'] == statenames[state]:
|
||||
return True
|
||||
return False
|
||||
|
||||
@property
|
||||
def stack_used(self):
|
||||
"""calculate the stack used by the thread"""
|
||||
if 'stack_used' not in self.__dict__:
|
||||
stack_base = self._tcb['stack_alloc_ptr'].cast(gdb.lookup_type('unsigned char').pointer())
|
||||
if stack_base == 0:
|
||||
self.__dict__['stack_used'] = 0
|
||||
else:
|
||||
stack_limit = self._tcb['adj_stack_size']
|
||||
for offset in range(0, stack_limit):
|
||||
if stack_base[offset] != 0xff:
|
||||
break
|
||||
self.__dict__['stack_used'] = stack_limit - offset
|
||||
return self.__dict__['stack_used']
|
||||
|
||||
@property
|
||||
def name(self):
|
||||
"""return the task's name"""
|
||||
return self._tcb['name'].string()
|
||||
|
||||
@property
|
||||
def state(self):
|
||||
"""return the name of the task's current state"""
|
||||
statenames = gdb.types.make_enum_dict(gdb.lookup_type('enum tstate_e'))
|
||||
for name,value in statenames.iteritems():
|
||||
if value == self._tcb['task_state']:
|
||||
return name
|
||||
return 'UNKNOWN'
|
||||
|
||||
@property
|
||||
def waiting_for(self):
|
||||
"""return a description of what the task is waiting for, if it is waiting"""
|
||||
if self._state_is('TSTATE_WAIT_SEM'):
|
||||
waitsem = self._tcb['waitsem'].dereference()
|
||||
waitsem_holder = waitsem['holder']
|
||||
holder = NX_task.for_tcb(waitsem_holder['htcb'])
|
||||
if holder is not None:
|
||||
return '{}({})'.format(waitsem.address, holder.name)
|
||||
else:
|
||||
return '{}(<bad holder>)'.format(waitsem.address)
|
||||
if self._state_is('TSTATE_WAIT_SIG'):
|
||||
return 'signal'
|
||||
return None
|
||||
|
||||
@property
|
||||
def is_waiting(self):
|
||||
"""tests whether the task is waiting for something"""
|
||||
if self._state_is('TSTATE_WAIT_SEM') or self._state_is('TSTATE_WAIT_SIG'):
|
||||
return True
|
||||
|
||||
@property
|
||||
def is_runnable(self):
|
||||
"""tests whether the task is runnable"""
|
||||
if (self._state_is('TSTATE_TASK_PENDING') or
|
||||
self._state_is('TSTATE_TASK_READYTORUN') or
|
||||
self._state_is('TSTATE_TASK_RUNNING')):
|
||||
return True
|
||||
return False
|
||||
|
||||
@property
|
||||
def file_descriptors(self):
|
||||
"""return a dictionary of file descriptors and inode pointers"""
|
||||
filelist = self._group['tg_filelist']
|
||||
filearray = filelist['fl_files']
|
||||
result = dict()
|
||||
for i in range(filearray.type.range()[0],filearray.type.range()[1]):
|
||||
inode = long(filearray[i]['f_inode'])
|
||||
if inode != 0:
|
||||
result[i] = inode
|
||||
return result
|
||||
|
||||
@property
|
||||
def registers(self):
|
||||
if 'registers' not in self.__dict__:
|
||||
registers = dict()
|
||||
if self._state_is('TSTATE_TASK_RUNNING'):
|
||||
# XXX need to fix this to handle interrupt context
|
||||
registers['R0'] = long(gdb.parse_and_eval('$r0'))
|
||||
registers['R1'] = long(gdb.parse_and_eval('$r1'))
|
||||
registers['R2'] = long(gdb.parse_and_eval('$r2'))
|
||||
registers['R3'] = long(gdb.parse_and_eval('$r3'))
|
||||
registers['R4'] = long(gdb.parse_and_eval('$r4'))
|
||||
registers['R5'] = long(gdb.parse_and_eval('$r5'))
|
||||
registers['R6'] = long(gdb.parse_and_eval('$r6'))
|
||||
registers['R7'] = long(gdb.parse_and_eval('$r7'))
|
||||
registers['R8'] = long(gdb.parse_and_eval('$r8'))
|
||||
registers['R9'] = long(gdb.parse_and_eval('$r9'))
|
||||
registers['R10'] = long(gdb.parse_and_eval('$r10'))
|
||||
registers['R11'] = long(gdb.parse_and_eval('$r11'))
|
||||
registers['R12'] = long(gdb.parse_and_eval('$r12'))
|
||||
registers['R13'] = long(gdb.parse_and_eval('$r13'))
|
||||
registers['SP'] = long(gdb.parse_and_eval('$sp'))
|
||||
registers['R14'] = long(gdb.parse_and_eval('$r14'))
|
||||
registers['LR'] = long(gdb.parse_and_eval('$lr'))
|
||||
registers['R15'] = long(gdb.parse_and_eval('$r15'))
|
||||
registers['PC'] = long(gdb.parse_and_eval('$pc'))
|
||||
registers['XPSR'] = long(gdb.parse_and_eval('$xpsr'))
|
||||
# this would only be valid if we were in an interrupt
|
||||
registers['EXC_RETURN'] = 0
|
||||
# we should be able to get this...
|
||||
registers['PRIMASK'] = 0
|
||||
else:
|
||||
context = self._tcb['xcp']
|
||||
regs = context['regs']
|
||||
registers['R0'] = long(regs[27])
|
||||
registers['R1'] = long(regs[28])
|
||||
registers['R2'] = long(regs[29])
|
||||
registers['R3'] = long(regs[30])
|
||||
registers['R4'] = long(regs[2])
|
||||
registers['R5'] = long(regs[3])
|
||||
registers['R6'] = long(regs[4])
|
||||
registers['R7'] = long(regs[5])
|
||||
registers['R8'] = long(regs[6])
|
||||
registers['R9'] = long(regs[7])
|
||||
registers['R10'] = long(regs[8])
|
||||
registers['R11'] = long(regs[9])
|
||||
registers['R12'] = long(regs[31])
|
||||
registers['R13'] = long(regs[0])
|
||||
registers['SP'] = long(regs[0])
|
||||
registers['R14'] = long(regs[32])
|
||||
registers['LR'] = long(regs[32])
|
||||
registers['R15'] = long(regs[33])
|
||||
registers['PC'] = long(regs[33])
|
||||
registers['XPSR'] = long(regs[34])
|
||||
registers['EXC_RETURN'] = long(regs[10])
|
||||
registers['PRIMASK'] = long(regs[1])
|
||||
self.__dict__['registers'] = registers
|
||||
return self.__dict__['registers']
|
||||
|
||||
def __repr__(self):
|
||||
return "<NX_task {}>".format(self.pid)
|
||||
|
||||
def __str__(self):
|
||||
return "{}:{}".format(self.pid, self.name)
|
||||
|
||||
def __format__(self, format_spec):
|
||||
return format_spec.format(
|
||||
pid = self.pid,
|
||||
name = self.name,
|
||||
state = self.state,
|
||||
waiting_for = self.waiting_for,
|
||||
stack_used = self.stack_used,
|
||||
stack_limit = self._tcb['adj_stack_size'],
|
||||
file_descriptors = self.file_descriptors,
|
||||
registers = self.registers
|
||||
)
|
||||
|
||||
class NX_show_task (gdb.Command):
|
||||
"""(NuttX) prints information about a task"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_task, self).__init__("show task", gdb.COMMAND_USER)
|
||||
|
||||
def invoke(self, arg, from_tty):
|
||||
t = NX_task.for_pid(int(arg))
|
||||
if t is not None:
|
||||
my_fmt = 'PID:{pid} name:{name} state:{state}\n'
|
||||
my_fmt += ' stack used {stack_used} of {stack_limit}\n'
|
||||
if t.is_waiting:
|
||||
my_fmt += ' waiting for {waiting_for}\n'
|
||||
my_fmt += ' open files: {file_descriptors}\n'
|
||||
my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n'
|
||||
my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n'
|
||||
my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n'
|
||||
my_fmt += ' R12 {registers[PC]:#010x}\n'
|
||||
my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n'
|
||||
print format(t, my_fmt)
|
||||
|
||||
class NX_show_tasks (gdb.Command):
|
||||
"""(NuttX) prints a list of tasks"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_tasks, self).__init__('show tasks', gdb.COMMAND_USER)
|
||||
|
||||
def invoke(self, args, from_tty):
|
||||
tasks = NX_task.tasks()
|
||||
for t in tasks:
|
||||
print format(t, '{pid:<2} {name:<16} {state:<20} {stack_used:>4}/{stack_limit:<4}')
|
||||
|
||||
NX_show_task()
|
||||
NX_show_tasks()
|
||||
|
||||
class NX_show_heap (gdb.Command):
|
||||
"""(NuttX) prints the heap"""
|
||||
|
||||
def __init__(self):
|
||||
super(NX_show_heap, self).__init__('show heap', gdb.COMMAND_USER)
|
||||
if gdb.lookup_type('struct mm_allocnode_s').sizeof == 8:
|
||||
self._allocflag = 0x80000000
|
||||
self._allocnodesize = 8
|
||||
else:
|
||||
self._allocflag = 0x8000
|
||||
self._allocnodesize = 4
|
||||
|
||||
def _node_allocated(self, allocnode):
|
||||
if allocnode['preceding'] & self._allocflag:
|
||||
return True
|
||||
return False
|
||||
|
||||
def _node_size(self, allocnode):
|
||||
return allocnode['size'] & ~self._allocflag
|
||||
|
||||
def _print_allocations(self, region_start, region_end):
|
||||
if region_start >= region_end:
|
||||
print 'heap region {} corrupt'.format(hex(region_start))
|
||||
return
|
||||
nodecount = region_end - region_start
|
||||
print 'heap {} - {}'.format(region_start, region_end)
|
||||
cursor = 1
|
||||
while cursor < nodecount:
|
||||
allocnode = region_start[cursor]
|
||||
if self._node_allocated(allocnode):
|
||||
state = ''
|
||||
else:
|
||||
state = '(free)'
|
||||
print ' {} {} {}'.format(allocnode.address + 8, self._node_size(allocnode), state)
|
||||
cursor += self._node_size(allocnode) / self._allocnodesize
|
||||
|
||||
def invoke(self, args, from_tty):
|
||||
heap = gdb.lookup_global_symbol('g_mmheap').value()
|
||||
nregions = heap['mm_nregions']
|
||||
region_starts = heap['mm_heapstart']
|
||||
region_ends = heap['mm_heapend']
|
||||
print "{} heap(s)".format(nregions)
|
||||
# walk the heaps
|
||||
for i in range(0, nregions):
|
||||
self._print_allocations(region_starts[i], region_ends[i])
|
||||
|
||||
NX_show_heap()
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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; ///<Various IO alarms
|
||||
|
||||
/* subscribed topics */
|
||||
int _t_actuators; ///< actuator controls topic
|
||||
int _t_actuators; ///< actuator controls topic
|
||||
int _t_actuator_armed; ///< system armed control topic
|
||||
int _t_vehicle_control_mode; ///< vehicle control mode topic
|
||||
int _t_param; ///<parameter update topic
|
||||
int _t_vehicle_control_mode;///< vehicle control mode topic
|
||||
int _t_param; ///< parameter update topic
|
||||
|
||||
/* advertised topics */
|
||||
orb_advert_t _to_input_rc; ///<rc inputs from IO topic
|
||||
orb_advert_t _to_actuators_effective; ///<effective actuator controls topic
|
||||
orb_advert_t _to_outputs; ///<mixed servo outputs topic
|
||||
orb_advert_t _to_battery; ///<battery status / voltage topic
|
||||
orb_advert_t _to_input_rc; ///< rc inputs from io
|
||||
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
|
||||
orb_advert_t _to_outputs; ///< mixed servo outputs topic
|
||||
orb_advert_t _to_battery; ///< battery status / voltage
|
||||
orb_advert_t _to_safety; ///< status of safety
|
||||
|
||||
actuator_outputs_s _outputs; ///<mixed outputs
|
||||
|
|
|
|||
|
|
@ -399,16 +399,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
}
|
||||
|
||||
case VEHICLE_CMD_NAV_TAKEOFF: {
|
||||
transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
|
||||
if (armed->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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
|
||||
* Author: Anton Babushkin <rk3dov@gmail.com>
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -1,8 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -35,7 +34,7 @@
|
|||
|
||||
/*
|
||||
* @file multirotor_pos_control_params.c
|
||||
*
|
||||
*
|
||||
* Parameters for multirotor_pos_control
|
||||
*/
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -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 <systemlib/param/param.h>
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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"),
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
Loading…
Reference in New Issue