mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 15:23:34 +08:00
Merge branch 'master' into new_state_machine
compiling again Conflicts: src/modules/fixedwing_att_control/fixedwing_att_control_att.c src/modules/fixedwing_att_control/fixedwing_att_control_rate.c src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c src/modules/mavlink/orb_listener.c src/modules/multirotor_att_control/multirotor_attitude_control.c src/modules/multirotor_att_control/multirotor_rate_control.c src/modules/systemlib/pid/pid.c src/modules/systemlib/pid/pid.h src/modules/uORB/objects_common.cpp
This commit is contained in:
Julian Oes
@@ -439,7 +439,7 @@ int multirotor_att_control_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1 + optioncount], "start")) {
|
||||
|
||||
thread_should_exit = false;
|
||||
mc_task = task_spawn("multirotor_att_control",
|
||||
mc_task = task_spawn_cmd("multirotor_att_control",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 15,
|
||||
2048,
|
||||
|
||||
@@ -1,12 +1,12 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,7 +39,15 @@
|
||||
|
||||
/*
|
||||
* @file multirotor_attitude_control.c
|
||||
* Implementation of attitude controller
|
||||
*
|
||||
* Implementation of attitude controller for multirotors.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "multirotor_attitude_control.h"
|
||||
@@ -129,16 +137,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
||||
static uint64_t last_run = 0;
|
||||
static uint64_t last_input = 0;
|
||||
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
last_run = hrt_absolute_time();
|
||||
|
||||
if (last_input != att_sp->timestamp) {
|
||||
last_input = att_sp->timestamp;
|
||||
}
|
||||
|
||||
// static int sensor_delay;
|
||||
// sensor_delay = hrt_absolute_time() - att->timestamp;
|
||||
|
||||
static int motor_skip_counter = 0;
|
||||
|
||||
static PID_t pitch_controller;
|
||||
@@ -156,8 +160,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
||||
parameters_init(&h);
|
||||
parameters_update(&h, &p);
|
||||
|
||||
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET);
|
||||
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET);
|
||||
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
|
||||
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
@@ -168,8 +172,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
||||
parameters_update(&h, &p);
|
||||
|
||||
/* apply parameters */
|
||||
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f);
|
||||
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f);
|
||||
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
|
||||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
|
||||
@@ -1,12 +1,12 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,7 +39,15 @@
|
||||
|
||||
/*
|
||||
* @file multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*
|
||||
* Definition of attitude controller for multirotors.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -36,10 +38,12 @@
|
||||
/**
|
||||
* @file multirotor_rate_control.c
|
||||
*
|
||||
* Implementation of rate controller
|
||||
* Implementation of rate controller for multirotors.
|
||||
*
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include "multirotor_rate_control.h"
|
||||
@@ -158,8 +162,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
|
||||
static uint64_t last_input = 0;
|
||||
|
||||
// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
|
||||
|
||||
if (last_input != rate_sp->timestamp) {
|
||||
last_input = rate_sp->timestamp;
|
||||
}
|
||||
@@ -187,8 +189,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
parameters_update(&h, &p);
|
||||
initialized = true;
|
||||
|
||||
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP);
|
||||
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP);
|
||||
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
|
||||
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
|
||||
|
||||
}
|
||||
|
||||
@@ -196,8 +198,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
if (motor_skip_counter % 2500 == 0) {
|
||||
/* update parameters from storage */
|
||||
parameters_update(&h, &p);
|
||||
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor);
|
||||
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor);
|
||||
pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
|
||||
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
|
||||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
@@ -208,31 +210,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
||||
|
||||
/* control pitch (forward) output */
|
||||
float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
|
||||
rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
|
||||
rates[1], 0.0f, deltaT,
|
||||
&control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d);
|
||||
|
||||
/* control roll (left/right) output */
|
||||
float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
|
||||
rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
|
||||
rates[0], 0.0f, deltaT,
|
||||
&control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(pitch_control)) {
|
||||
pitch_control_last = pitch_control;
|
||||
|
||||
} else {
|
||||
pitch_control = pitch_control_last;
|
||||
warnx("rej. NaN ctrl pitch");
|
||||
}
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(roll_control)) {
|
||||
roll_control_last = roll_control;
|
||||
|
||||
} else {
|
||||
roll_control = roll_control_last;
|
||||
warnx("rej. NaN ctrl roll");
|
||||
}
|
||||
|
||||
/* control yaw rate */ //XXX use library here and use rates_acc[2]
|
||||
/* control yaw rate */ //XXX use library here
|
||||
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
|
||||
@@ -1,12 +1,12 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -39,7 +39,15 @@
|
||||
|
||||
/*
|
||||
* @file multirotor_attitude_control.h
|
||||
* Attitude control for multi rotors.
|
||||
*
|
||||
* Definition of rate controller for multirotors.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Laurens Mackay <mackayl@student.ethz.ch>
|
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch>
|
||||
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef MULTIROTOR_RATE_CONTROL_H_
|
||||
|
||||
Reference in New Issue
Block a user