fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added deamon example, reworked / merged multirotor attitude control. Ready for AR.Drone interface changes and integration tests

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-19 15:52:59 +02:00
parent 85bc4f683a
commit dae0b922f1
22 changed files with 399 additions and 565 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/Makefile
+1 -1
View File
@@ -40,6 +40,6 @@ PRIORITY = SCHED_PRIORITY_MAX - 15
STACKSIZE = 4096
# explicit list of sources - not everything is built currently
CSRCS = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
CSRCS = ardrone_control.c ardrone_motor_control.c rate_control.c attitude_control.c pid.c
include $(APPDIR)/mk/app.mk
apps/ardrone_control/ardrone_control.c
+5 -3
View File
@@ -62,6 +62,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/ardrone_motors_setpoint.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
__EXPORT int ardrone_control_main(int argc, char *argv[]);
@@ -181,6 +182,8 @@ int ardrone_control_main(int argc, char *argv[])
memset(&raw, 0, sizeof(raw));
struct ardrone_motors_setpoint_s setpoint;
memset(&setpoint, 0, sizeof(setpoint));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -251,9 +254,8 @@ int ardrone_control_main(int argc, char *argv[])
float roll_control, pitch_control, yaw_control, thrust_control;
multirotor_control_attitude(&att_sp, &att, &state, motor_test_mode,
&roll_control, &pitch_control, &yaw_control, &thrust_control);
ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, motor_test_mode);
multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
} else {
/* invalid mode, complain */
apps/ardrone_control/attitude_control.c
+13 -7
View File
@@ -56,8 +56,9 @@
#define MAX_MOTOR_COUNT 16
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output)
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
struct actuator_controls_s *actuators, bool verbose)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -218,13 +219,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
roll_controller.saturated = 1;
}
*roll_output = roll_control;
*pitch_output = pitch_control;
*yaw_output = yaw_rate_control;
*thrust_output = motor_thrust;
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
}
void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) {
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) {
float roll_control = actuators->control[0];
float pitch_control = actuators->control[1];
float yaw_control = actuators->control[2];
float motor_thrust = actuators->control[3];
unsigned int motor_skip_counter = 0;
apps/ardrone_control/attitude_control.h
+3 -3
View File
@@ -51,11 +51,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/ardrone_control.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
const struct vehicle_status_s *status, bool verbose,
float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output);
const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose);
void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose);
void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
#endif /* ATTITUDE_CONTROL_H_ */
apps/ardrone_control/rate_control.c
-1
View File
@@ -39,7 +39,6 @@
*/
#include "rate_control.h"
#include "ardrone_control_helper.h"
#include "ardrone_motor_control.h"
#include <arch/board/up_hrt.h>