dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'pid_fixes' into new_state_machine 

Conflicts:
	src/modules/multirotor_att_control/multirotor_rate_control.c
	src/modules/sdlog2/sdlog2.c
	src/modules/sdlog2/sdlog2_messages.h

and some fixes, logging of control PID values now working
This commit is contained in:
Julian Oes 2013-06-16 17:14:22 +02:00
parent bd7f86bb6a 216617431d
commit bca60b98bd
7 changed files with 42 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/multirotor_att_control/multirotor_att_control_main.c
View File

@ -64,6 +64,7 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_debug.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
@ -84,7 +85,7 @@ static int mc_task;
static bool motor_test_mode = false;
static orb_advert_t actuator_pub;
static orb_advert_t control_debug_pub;
static int
@ -111,6 +112,9 @@ mc_thread_main(int argc, char *argv[])
struct actuator_controls_s actuators;
memset(&actuators, 0, sizeof(actuators));
struct vehicle_control_debug_s control_debug;
memset(&control_debug, 0, sizeof(control_debug));
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
@ -138,6 +142,8 @@ mc_thread_main(int argc, char *argv[])
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
actuators.control[i] = 0.0f;
}
control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
@ -380,7 +386,7 @@ mc_thread_main(int argc, char *argv[])
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
@ -403,9 +409,11 @@ mc_thread_main(int argc, char *argv[])
gyro[1] = att.pitchspeed;
gyro[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, gyro, &actuators);
multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
/* update control_mode */
flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
flag_control_manual_enabled = control_mode.flag_control_manual_enabled;

7
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@ -123,7 +123,8 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
}
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@ -182,11 +183,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL);
att->pitch, att->pitchspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
att->roll, att->rollspeed, deltaT, NULL, NULL, NULL);
att->roll, att->rollspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d);
// printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT);

5
src/modules/multirotor_att_control/multirotor_attitude_control.h
View File

@ -50,8 +50,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */

17
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@ -55,7 +55,7 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_control_debug.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
@ -151,7 +151,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
}
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@ -178,10 +179,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
static bool initialized = false;
static struct vehicle_control_debug_s control_debug;
static orb_advert_t control_debug_pub;
/* initialize the pid controllers when the function is called for the first time */
if (initialized == false) {
@ -194,7 +191,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
}
/* load new parameters with lower rate */
@ -207,11 +203,11 @@ 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)) {
@ -246,7 +242,4 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
printf("Published control debug\n");
}

4
src/modules/multirotor_att_control/multirotor_rate_control.h
View File

@ -49,8 +49,10 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_debug.h>
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators);
const float rates[], struct actuator_controls_s *actuators,
const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */

22
src/modules/sdlog2/sdlog2.c
View File

@ -91,7 +91,6 @@
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
printf("size: %d\n", LOG_PACKET_SIZE(_msg)); \
} else { \
log_msgs_skipped++; \
/*printf("skip\n");*/ \
@ -616,6 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 17;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@ -1072,26 +1072,20 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- CONTROL DEBUG --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug);
printf("copied control debug\n");
// log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
// log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
// log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
log_msg.msg_type = LOG_CTRL_MSG;
log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p;
log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i;
log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d;
log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p;
log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i;
log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d;
// log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
// log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
// log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p;
log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i;
log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d;
log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p;
log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i;
log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d;
// log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p;
// log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i;
// log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d;
log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p;
log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i;
log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d;
LOGBUFFER_WRITE_AND_COUNT(CTRL);
}

25
src/modules/sdlog2/sdlog2_messages.h
View File

@ -159,29 +159,18 @@ struct log_STAT_s {
/* --- CTRL - CONTROL DEBUG --- */
#define LOG_CTRL_MSG 11
struct log_CTRL_s {
// float roll_p;
// float roll_i;
// float roll_d;
float roll_p;
float roll_i;
float roll_d;
float roll_rate_p;
float roll_rate_i;
float roll_rate_d;
// float pitch_p;
// float pitch_i;
// float pitch_d;
float pitch_p;
float pitch_i;
float pitch_d;
float pitch_rate_p;
float pitch_rate_i;
float pitch_rate_d;
// float yaw_p;
// float yaw_i;
// float yaw_d;
float yaw_rate_p;
float yaw_rate_i;
float yaw_rate_d;
};
/* --- RC - RC INPUT CHANNELS --- */
@ -210,7 +199,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"),
LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"),
LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
};