fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 04:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged release_v1.0.0 into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-30 15:30:45 +02:00
parent 454becdae5 3ef6212155
commit 50ba1f7365
23 changed files with 1085 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+25 -16
View File
@@ -122,6 +122,7 @@ private:
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
int _vehicle_status_sub; /**< vehicle status subscription */
int _att_sub; /**< vehicle attitude subscription */
int _att_sp_sub; /**< vehicle attitude setpoint */
int _control_mode_sub; /**< vehicle control mode subscription */
@@ -137,6 +138,7 @@ private:
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
@@ -319,6 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_reset_alt_sp(true),
_mode_auto(false)
{
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
@@ -474,6 +477,12 @@ MulticopterPositionControl::poll_subscriptions()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
orb_check(_att_sub, &updated);
if (updated) {
@@ -903,6 +912,7 @@ MulticopterPositionControl::task_main()
/*
* do subscriptions
*/
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -1373,44 +1383,44 @@ MulticopterPositionControl::task_main()
reset_int_xy = true;
}
// generate attitude setpoint from manual controls
/* generate attitude setpoint from manual controls */
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) {
// reset yaw setpoint to current position if needed
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
}
// do not move yaw while arming
/* do not move yaw while arming */
else if (_manual.z > 0.1f)
{
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw);
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX);
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX);
// If the yaw offset became too big for the system to track stop
// shifting it
if (fabsf(yaw_offs) < yaw_offset_max) {
_att_sp.yaw_body = yaw_target;
}
}
//Control roll and pitch directly if we no aiding velocity controller is active
/* control roll and pitch directly if we no aiding velocity controller is active */
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
}
//Construct attitude setpoint rotation matrix
/* construct attitude setpoint rotation matrix */
math::Matrix<3,3> R_sp;
R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
@@ -1435,10 +1445,9 @@ MulticopterPositionControl::task_main()
if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr) {
if (_att_sp_pub != nullptr && _vehicle_status.is_rotary_wing) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
} else if (_att_sp_pub <= 0 && _vehicle_status.is_rotary_wing){
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
src/modules/mc_pos_control/mc_pos_control_params.c
+8 -9
View File
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Anton Babushkin <anton.babushkin@me.com>
* Copyright (c) 2013-2015 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
@@ -36,7 +35,7 @@
* @file mc_pos_control_params.c
* Multicopter position controller parameters.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Anton Babushkin <anton@px4.io>
*/
#include <systemlib/param/param.h>
@@ -107,7 +106,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
*
* @unit m/s
* @min 0.0
* @max 8 m/s
* @max 8.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
@@ -184,7 +183,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
*
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@@ -196,7 +195,7 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f);
*
* Limits maximum tilt angle on landing.
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@@ -215,7 +214,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f);
/**
* Max manual roll
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@@ -225,7 +224,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
*
* @unit deg
* @unit degree
* @min 0.0
* @max 90.0
* @group Multicopter Position Control
@@ -235,7 +234,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
*
* @unit deg/s
* @unit degree / s
* @min 0.0
* @group Multicopter Position Control
*/