forked from Archive/PX4-Autopilot
Fixed code style for multirotor_att_control app
This commit is contained in:
parent
cf563eda86
commit
cded2787f0
|
@ -125,9 +125,9 @@ mc_thread_main(int argc, char *argv[])
|
|||
* orb_set_interval(att_sub, 5);
|
||||
*/
|
||||
struct pollfd fds[2] = {
|
||||
{ .fd = att_sub, .events = POLLIN },
|
||||
{ .fd = param_sub, .events = POLLIN }
|
||||
};
|
||||
{ .fd = att_sub, .events = POLLIN },
|
||||
{ .fd = param_sub, .events = POLLIN }
|
||||
};
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
|
||||
|
@ -171,6 +171,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
if (ret < 0) {
|
||||
/* poll error, count it in perf */
|
||||
perf_count(mc_err_perf);
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* no return value, ignore */
|
||||
} else {
|
||||
|
@ -193,9 +194,11 @@ mc_thread_main(int argc, char *argv[])
|
|||
/* get a local copy of system state */
|
||||
bool updated;
|
||||
orb_check(state_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
}
|
||||
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
|
@ -204,9 +207,11 @@ mc_thread_main(int argc, char *argv[])
|
|||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
|
||||
/* get a local copy of rates setpoint */
|
||||
orb_check(setpoint_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
|
||||
}
|
||||
|
||||
/* get a local copy of the current sensor values */
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
|
||||
|
||||
|
@ -222,6 +227,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
// printf("thrust_rate=%8.4f\n",offboard_sp.p4);
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
|
||||
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
|
||||
att_sp.roll_body = offboard_sp.p1;
|
||||
att_sp.pitch_body = offboard_sp.p2;
|
||||
|
@ -240,8 +246,8 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
/* initialize to current yaw if switching to manual or att control */
|
||||
if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
|
||||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
state.flag_system_armed != flag_system_armed) {
|
||||
state.flag_control_manual_enabled != flag_control_manual_enabled ||
|
||||
state.flag_system_armed != flag_system_armed) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
}
|
||||
|
||||
|
@ -264,6 +270,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
*/
|
||||
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
|
||||
att_sp.thrust = failsafe_throttle;
|
||||
|
||||
} else {
|
||||
att_sp.thrust = 0.0f;
|
||||
}
|
||||
|
@ -290,11 +297,12 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
|
||||
if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
|
||||
if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
|
||||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* This mode SHOULD be the default mode, which is:
|
||||
|
@ -309,11 +317,13 @@ mc_thread_main(int argc, char *argv[])
|
|||
rates_sp.yaw = manual.yaw;
|
||||
control_yaw_position = false;
|
||||
first_time_after_yaw_speed_control = true;
|
||||
|
||||
} else {
|
||||
if (first_time_after_yaw_speed_control) {
|
||||
att_sp.yaw_body = att.yaw;
|
||||
first_time_after_yaw_speed_control = false;
|
||||
}
|
||||
|
||||
control_yaw_position = true;
|
||||
}
|
||||
}
|
||||
|
@ -340,7 +350,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
} else {
|
||||
/* manual rate inputs, from RC control or joystick */
|
||||
if (state.flag_control_rates_enabled &&
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
|
||||
state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
|
||||
rates_sp.roll = manual.roll;
|
||||
|
||||
rates_sp.pitch = manual.pitch;
|
||||
|
@ -354,9 +364,9 @@ mc_thread_main(int argc, char *argv[])
|
|||
|
||||
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
|
||||
if (state.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);
|
||||
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
|
||||
}
|
||||
|
||||
/* measure in what intervals the controller runs */
|
||||
|
@ -367,6 +377,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
/* get current rate setpoint */
|
||||
bool rates_sp_valid = false;
|
||||
orb_check(rates_sp_sub, &rates_sp_valid);
|
||||
|
||||
if (rates_sp_valid) {
|
||||
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
|
||||
}
|
||||
|
@ -394,6 +405,7 @@ mc_thread_main(int argc, char *argv[])
|
|||
/* kill all outputs */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
|
||||
|
@ -415,6 +427,7 @@ usage(const char *reason)
|
|||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
|
||||
fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
|
||||
fprintf(stderr, " <mode> is 'rates' or 'attitude'\n");
|
||||
fprintf(stderr, " -t enables motor test mode with 10%% thrust\n");
|
||||
|
@ -432,22 +445,25 @@ int multirotor_att_control_main(int argc, char *argv[])
|
|||
motor_test_mode = true;
|
||||
optioncount += 1;
|
||||
break;
|
||||
|
||||
case ':':
|
||||
usage("missing parameter");
|
||||
break;
|
||||
|
||||
default:
|
||||
fprintf(stderr, "option: -%c\n", ch);
|
||||
usage("unrecognized option");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
argc -= optioncount;
|
||||
//argv += optioncount;
|
||||
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1+optioncount], "start")) {
|
||||
if (!strcmp(argv[1 + optioncount], "start")) {
|
||||
|
||||
thread_should_exit = false;
|
||||
mc_task = task_spawn("multirotor_att_control",
|
||||
|
@ -459,7 +475,7 @@ int multirotor_att_control_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1+optioncount], "stop")) {
|
||||
if (!strcmp(argv[1 + optioncount], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
|
|
@ -158,16 +158,18 @@ 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)
|
||||
{
|
||||
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;
|
||||
|
||||
|
@ -189,9 +191,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
|||
parameters_update(&h, &p);
|
||||
|
||||
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
1000.0f, PID_MODE_DERIVATIV_SET);
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
@ -207,7 +209,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
|||
}
|
||||
|
||||
/* reset integral if on ground */
|
||||
if(att_sp->thrust < 0.1f) {
|
||||
if (att_sp->thrust < 0.1f) {
|
||||
pid_reset_integral(&pitch_controller);
|
||||
pid_reset_integral(&roll_controller);
|
||||
}
|
||||
|
@ -217,13 +219,13 @@ 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);
|
||||
att->pitch, att->pitchspeed, deltaT);
|
||||
|
||||
/* control roll (left/right) output */
|
||||
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
|
||||
att->roll, att->rollspeed, deltaT);
|
||||
att->roll, att->rollspeed, deltaT);
|
||||
|
||||
if(control_yaw_position) {
|
||||
if (control_yaw_position) {
|
||||
/* control yaw rate */
|
||||
|
||||
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
|
||||
|
@ -233,12 +235,14 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
|
|||
|
||||
if (yaw_error > M_PI_F) {
|
||||
yaw_error -= M_TWOPI_F;
|
||||
|
||||
} else if (yaw_error < -M_PI_F) {
|
||||
yaw_error += M_TWOPI_F;
|
||||
}
|
||||
|
||||
rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
|
||||
}
|
||||
|
||||
rates_sp->thrust = att_sp->thrust;
|
||||
|
||||
motor_skip_counter++;
|
||||
|
|
|
@ -52,6 +52,6 @@
|
|||
#include <uORB/topics/actuator_controls.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);
|
||||
|
||||
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
|
||||
|
|
|
@ -148,7 +148,7 @@ 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)
|
||||
{
|
||||
static float roll_control_last = 0;
|
||||
static float pitch_control_last = 0;
|
||||
|
@ -157,6 +157,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
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;
|
||||
}
|
||||
|
@ -189,9 +190,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
/* control pitch (forward) output */
|
||||
float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(pitch_control)) {
|
||||
pitch_control_last = pitch_control;
|
||||
|
||||
} else {
|
||||
pitch_control = 0.0f;
|
||||
warnx("rej. NaN ctrl pitch");
|
||||
|
@ -199,9 +202,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
/* control roll (left/right) output */
|
||||
float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (isfinite(roll_control)) {
|
||||
roll_control_last = roll_control;
|
||||
|
||||
} else {
|
||||
roll_control = 0.0f;
|
||||
warnx("rej. NaN ctrl roll");
|
||||
|
@ -209,6 +214,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
|
|||
|
||||
/* control yaw rate */
|
||||
float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
|
||||
|
||||
/* increase resilience to faulty control inputs */
|
||||
if (!isfinite(yaw_rate_control)) {
|
||||
yaw_rate_control = 0.0f;
|
||||
|
|
|
@ -51,6 +51,6 @@
|
|||
#include <uORB/topics/actuator_controls.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);
|
||||
|
||||
#endif /* MULTIROTOR_RATE_CONTROL_H_ */
|
||||
|
|
Loading…
Reference in New Issue