diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-08 10:37:01 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-08 10:37:01 +0100 |
commit | 87df7c3243412d4fc7a0c40967b2abe078f7a0b2 (patch) | |
tree | 63e442c07a3e709ab76f456f366f437c0552d700 /src/modules | |
parent | 65629d09d5e2424c6fcaf261c95f6d6995c4afd7 (diff) | |
download | px4-firmware-87df7c3243412d4fc7a0c40967b2abe078f7a0b2.tar.gz px4-firmware-87df7c3243412d4fc7a0c40967b2abe078f7a0b2.tar.bz2 px4-firmware-87df7c3243412d4fc7a0c40967b2abe078f7a0b2.zip |
move vehicle_attitude_setpoint to msg format
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_base.cpp | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_base.cpp | 4 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_base.h | 7 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 6 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_attitude_setpoint.h | 59 |
7 files changed, 41 insertions, 62 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp index 46fef3e67..f543c02f9 100644 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -31,7 +31,7 @@ /** * @file mc_att_control_base.cpp - * + * * @author Roman Bapst <bapstr@ethz.ch> * */ @@ -40,7 +40,6 @@ #include <math.h> #include <mathlib/mathlib.h> #include <drivers/drv_hrt.h> -#include <systemlib/err.h> using namespace std; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..cb55a25aa 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); - mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index 5e65e6508..f6b3268b5 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -172,7 +172,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ @@ -180,7 +180,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt) _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 1c2f3fb6f..c1ea52f63 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -34,7 +34,7 @@ /** * @file mc_att_control_base.h - * + * * MC Attitude Controller * * @author Tobias Naegeli <naegelit@student.ethz.ch> @@ -45,23 +45,20 @@ * @author Roman Bapst <bapstr@ethz.ch> * */ - +#include <px4.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include <unistd.h> #include <errno.h> #include <math.h> -#include <drivers/drv_hrt.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/actuator_armed.h> -#include <systemlib/err.h> #include <systemlib/perf_counter.h> #include <lib/mathlib/mathlib.h> 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 887a53508..c0e8957e0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -52,8 +52,7 @@ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ -#include <nuttx/config.h> -#include <stdio.h> +#include <px4.h> #include <stdlib.h> #include <string.h> #include <unistd.h> @@ -62,8 +61,6 @@ #include <poll.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> @@ -78,6 +75,7 @@ #include <systemlib/circuit_breaker.h> #include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> + #include "mc_att_control_base.h" /** 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 cea847603..4ea5fdfb6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -43,8 +43,9 @@ * @author Anton Babushkin <anton.babushkin@me.com> */ -#include <nuttx/config.h> -#include <stdio.h> +#include <px4.h> +#include <functional> +#include <cstdio> #include <stdlib.h> #include <string.h> #include <unistd.h> @@ -54,8 +55,7 @@ #include <poll.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> + #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/vehicle_rates_setpoint.h> @@ -67,8 +67,8 @@ #include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <systemlib/param/param.h> -#include <systemlib/err.h> +// #include <systemlib/param/param.h> +// #include <systemlib/err.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> #include <lib/geo/geo.h> @@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp() if (_reset_pos_sp) { _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ - _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0) + _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - _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) + _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_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: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } @@ -987,7 +987,7 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; @@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main() } /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ @@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main() R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ - memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a503aa0c6..29fb104df 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * 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 @@ -32,49 +31,37 @@ * ****************************************************************************/ -/** - * @file vehicle_attitude_setpoint.h - * Definition of the vehicle attitude setpoint uORB topic. - */ + /* Auto-generated by genmsg_cpp from file /home/thomasgubler/src/catkin_ws/src/Firmware/msg/px4_msgs/vehicle_attitude_setpoint.msg */ -#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ -#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_ + +#pragma once #include <stdint.h> -#include <stdbool.h> -#include <platforms/px4_defines.h> +#include "../uORB.h" + + /** * @addtogroup topics * @{ */ -/** - * vehicle attitude setpoint. - */ -struct vehicle_attitude_setpoint_s { - uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - - float roll_body; /**< body angle in NED frame */ - float pitch_body; /**< body angle in NED frame */ - float yaw_body; /**< body angle in NED frame */ - //float body_valid; /**< Set to true if body angles are valid */ - - float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ - bool R_valid; /**< Set to true if rotation matrix is valid */ - - //! For quaternion-based attitude control - float q_d[4]; /** Desired quaternion for quaternion control */ - bool q_d_valid; /**< Set to true if quaternion vector is valid */ - float q_e[4]; /** Attitude error in quaternion */ - bool q_e_valid; /**< Set to true if quaternion error vector is valid */ - - float thrust; /**< Thrust in Newton the power system should generate */ - - bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ - bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ - bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ +struct vehicle_attitude_setpoint_s { + uint64_t timestamp; + float roll_body; + float pitch_body; + float yaw_body; + float R_body[9]; + bool R_valid; + float q_d[4]; + bool q_d_valid; + float q_e[4]; + bool q_e_valid; + float thrust; + bool roll_reset_integral; + bool pitch_reset_integral; + bool yaw_reset_integral; }; /** @@ -83,5 +70,3 @@ struct vehicle_attitude_setpoint_s { /* register this as object request broker structure */ ORB_DECLARE(vehicle_attitude_setpoint); - -#endif /* TOPIC_ARDRONE_CONTROL_H_ */ |