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

vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed

This commit is contained in:
Anton Babushkin 2013-08-19 08:43:16 +02:00
parent ffc2a8b7a3
commit 3370ceceaf
7 changed files with 29 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/commander/commander.cpp
View File

@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[])
}
/* publish control mode */
if (navigation_state_changed) {
if (navigation_state_changed || arming_state_changed) {
/* publish new navigation state */
control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
control_mode.counter++;
control_mode.timestamp = t1;
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);

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

@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[])
perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
/* welcome user */
printf("[multirotor_att_control] starting\n");
warnx("starting");
/* store last control mode to detect mode switches */
bool flag_control_manual_enabled = false;
bool flag_control_attitude_enabled = false;
/* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
/* store if we stopped a yaw movement */
bool reset_yaw_sp = true;
/* prepare the handle for the failsafe throttle */
@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
/** STEP 1: Define which input is the dominating control input */
/* define which input is the dominating control input */
if (control_mode.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[])
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
// 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);
@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[])
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
// printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (control_mode.flag_control_manual_enabled) {
/* direct manual input */
if (control_mode.flag_control_attitude_enabled) {
@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[])
* multicopter (throttle = 0) does not make it jump up in the air
* if shutting down his remote.
*/
if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle
/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.thrust = failsafe_throttle;
@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = att.yaw;
reset_yaw_sp = false;
}
control_yaw_position = true;
}
@ -347,22 +339,25 @@ 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);
/* check if we should we reset integrals */
bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle
/* run attitude controller if needed */
if (control_mode.flag_control_attitude_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
/* run rates controller if needed */
if (control_mode.flag_control_rates_enabled) {
/* get current rate setpoint */
bool rates_sp_valid = false;
orb_check(rates_sp_sub, &rates_sp_valid);
bool rates_sp_updated = false;
orb_check(rates_sp_sub, &rates_sp_updated);
if (rates_sp_valid) {
if (rates_sp_updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
}
@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[])
rates[0] = att.rollspeed;
rates[1] = att.pitchspeed;
rates[2] = att.yawspeed;
multirotor_control_rates(&rates_sp, rates, &actuators);
multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
} else {
/* rates controller disabled, set actuators to zero for safety */
actuators.control[0] = 0.0f;
@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[])
} /* end of poll return value check */
}
printf("[multirotor att control] stopping, disarming motors.\n");
warnx("stopping, disarming motors");
/* kill all outputs */
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)

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

@ -166,7 +166,7 @@ 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, bool reset_integral)
{
static uint64_t last_run = 0;
static uint64_t last_input = 0;
@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
}
/* reset integral if on ground */
if (att_sp->thrust < 0.1f) {
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_controller);
pid_reset_integral(&roll_controller);
//TODO pid_reset_integral(&yaw_controller);
}
/* calculate current control outputs */
/* control pitch (forward) output */
@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
if (control_yaw_position) {
/* control yaw rate */
// TODO use pid lib
/* positive error: rotate to right, negative error, rotate to left (NED frame) */
// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);

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

@ -60,6 +60,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, bool reset_integral);
#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */

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

@ -152,7 +152,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, bool reset_integral)
{
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
}
/* reset integral if on ground */
if (rate_sp->thrust < 0.01f) {
/* reset integrals if needed */
if (reset_integral) {
pid_reset_integral(&pitch_rate_controller);
pid_reset_integral(&roll_rate_controller);
// TODO pid_reset_integral(&yaw_rate_controller);
}
/* control pitch (forward) output */

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

@ -59,6 +59,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, bool reset_integral);
#endif /* MULTIROTOR_RATE_CONTROL_H_ */

2
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -64,6 +64,8 @@ struct vehicle_control_mode_s
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
bool flag_armed;
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
// XXX needs yet to be set by state machine helper