aboutsummaryrefslogtreecommitdiff
path: root/src/modules/multirotor_att_control/multirotor_att_control_main.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/multirotor_att_control/multirotor_att_control_main.c')
-rw-r--r--src/modules/multirotor_att_control/multirotor_att_control_main.c222
1 files changed, 104 insertions, 118 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9..c057ef364 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -57,7 +57,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
@@ -82,15 +82,12 @@ static bool thread_should_exit;
static int mc_task;
static bool motor_test_mode = false;
-static orb_advert_t actuator_pub;
-
-static struct vehicle_status_s state;
-
static int
mc_thread_main(int argc, char *argv[])
{
/* declare and safely initialize all structs */
- memset(&state, 0, sizeof(state));
+ struct vehicle_control_mode_s control_mode;
+ memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -112,7 +109,7 @@ mc_thread_main(int argc, char *argv[])
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
- int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -134,7 +131,7 @@ mc_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f;
}
- actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+ orb_advert_t 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);
orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
@@ -145,18 +142,14 @@ 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;
- bool flag_system_armed = false;
- /* store if yaw position or yaw speed has been changed */
bool control_yaw_position = true;
-
- /* store if we stopped a yaw movement */
- bool first_time_after_yaw_speed_control = true;
+ bool reset_yaw_sp = true;
/* prepare the handle for the failsafe throttle */
param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
@@ -193,10 +186,10 @@ mc_thread_main(int argc, char *argv[])
/* get a local copy of system state */
bool updated;
- orb_check(state_sub, &updated);
+ orb_check(control_mode_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
/* get a local copy of manual setpoint */
@@ -215,16 +208,14 @@ 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 */
- if (state.flag_control_offboard_enabled) {
+ /* 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) {
rates_sp.roll = offboard_sp.p1;
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);
@@ -233,46 +224,49 @@ 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 (state.flag_control_manual_enabled) {
-
- if (state.flag_control_attitude_enabled) {
-
+ } else if (control_mode.flag_control_manual_enabled) {
+ /* direct manual input */
+ if (control_mode.flag_control_attitude_enabled) {
+ /* control attitude, update attitude setpoint depending on mode */
/* 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) {
+ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled ||
+ control_mode.flag_control_manual_enabled != flag_control_manual_enabled) {
att_sp.yaw_body = att.yaw;
}
static bool rc_loss_first_time = true;
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
- if (state.rc_signal_lost) {
- /* 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.roll_body = 0.0f;
- att_sp.pitch_body = 0.0f;
-
- /*
- * Only go to failsafe throttle if last known throttle was
- * high enough to create some lift to make hovering state likely.
- *
- * This is to prevent that someone landing, but not disarming his
- * 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) {
- att_sp.thrust = failsafe_throttle;
+ if (control_mode.failsave_highlevel) {
+ if (!control_mode.flag_control_velocity_enabled) {
+ /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */
+ att_sp.roll_body = 0.0f;
+ att_sp.pitch_body = 0.0f;
+
+ if (!control_mode.flag_control_climb_rate_enabled) {
+ /* Don't touch throttle in modes with altitude hold, it's handled by position controller.
+ *
+ * Only go to failsafe throttle if last known throttle was
+ * high enough to create some lift to make hovering state likely.
+ *
+ * This is to prevent that someone landing, but not disarming his
+ * 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) { // 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;
- } else {
- att_sp.thrust = 0.0f;
+ } else {
+ att_sp.thrust = 0.0f;
+ }
+ }
}
/* keep current yaw, do not attempt to go to north orientation,
@@ -287,55 +281,40 @@ mc_thread_main(int argc, char *argv[])
} else {
rc_loss_first_time = true;
- att_sp.roll_body = manual.roll;
- att_sp.pitch_body = manual.pitch;
-
- /* set attitude if arming */
- if (!flag_control_attitude_enabled && state.flag_system_armed) {
- att_sp.yaw_body = att.yaw;
+ /* control yaw in all manual / assisted modes */
+ /* set yaw if arming or switching to attitude stabilized mode */
+ if (!flag_control_attitude_enabled) {
+ reset_yaw_sp = true;
}
- /* 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) {
+ /* only move setpoint if manual input is != 0 */
+ if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { // TODO use landed status instead of throttle
+ rates_sp.yaw = manual.yaw;
+ control_yaw_position = false;
+ reset_yaw_sp = true;
- 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:
- * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
- *
- * However, we fall back to this setting for all other (nonsense)
- * settings as well.
- */
-
- /* only move setpoint if manual input is != 0 */
- if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
- rates_sp.yaw = manual.yaw;
- control_yaw_position = false;
- first_time_after_yaw_speed_control = true;
+ } else {
+ if (reset_yaw_sp) {
+ att_sp.yaw_body = att.yaw;
+ reset_yaw_sp = false;
+ }
+ control_yaw_position = true;
+ }
- } else {
- if (first_time_after_yaw_speed_control) {
- att_sp.yaw_body = att.yaw;
- first_time_after_yaw_speed_control = false;
- }
+ if (!control_mode.flag_control_velocity_enabled) {
+ /* don't update attitude setpoint in position control mode */
+ att_sp.roll_body = manual.roll;
+ att_sp.pitch_body = manual.pitch;
- control_yaw_position = true;
- }
+ if (!control_mode.flag_control_climb_rate_enabled) {
+ /* don't set throttle in altitude hold modes */
+ att_sp.thrust = manual.throttle;
}
}
- att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
- /* STEP 2: publish the controller output */
- orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-
if (motor_test_mode) {
printf("testmode");
att_sp.roll_body = 0.0f;
@@ -343,64 +322,71 @@ mc_thread_main(int argc, char *argv[])
att_sp.yaw_body = 0.0f;
att_sp.thrust = 0.1f;
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);
}
+ /* STEP 2: publish the controller output */
+ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
} else {
- /* manual rate inputs, from RC control or joystick */
- if (state.flag_control_rates_enabled &&
- state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+ /* manual rate inputs (ACRO), from RC control or joystick */
+ if (control_mode.flag_control_rates_enabled) {
rates_sp.roll = manual.roll;
-
rates_sp.pitch = manual.pitch;
rates_sp.yaw = manual.yaw;
rates_sp.thrust = manual.throttle;
rates_sp.timestamp = hrt_absolute_time();
}
}
-
}
- /** 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);
+ /* 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);
- float gyro[3];
+ /* run rates controller if needed */
+ if (control_mode.flag_control_rates_enabled) {
+ /* get current rate setpoint */
+ bool rates_sp_updated = false;
+ orb_check(rates_sp_sub, &rates_sp_updated);
- /* get current rate setpoint */
- bool rates_sp_valid = false;
- orb_check(rates_sp_sub, &rates_sp_valid);
+ if (rates_sp_updated) {
+ orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+ }
- if (rates_sp_valid) {
- orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+ /* apply controller */
+ float rates[3];
+ rates[0] = att.rollspeed;
+ rates[1] = att.pitchspeed;
+ rates[2] = att.yawspeed;
+ multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+ } else {
+ /* rates controller disabled, set actuators to zero for safety */
+ actuators.control[0] = 0.0f;
+ actuators.control[1] = 0.0f;
+ actuators.control[2] = 0.0f;
+ actuators.control[3] = 0.0f;
}
-
- /* apply controller */
- gyro[0] = att.rollspeed;
- gyro[1] = att.pitchspeed;
- gyro[2] = att.yawspeed;
-
- multirotor_control_rates(&rates_sp, gyro, &actuators);
+ actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
/* update state */
- flag_control_attitude_enabled = state.flag_control_attitude_enabled;
- flag_control_manual_enabled = state.flag_control_manual_enabled;
- flag_system_armed = state.flag_system_armed;
+ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled;
+ flag_control_manual_enabled = control_mode.flag_control_manual_enabled;
perf_end(mc_loop_perf);
} /* end of poll call for attitude updates */
} /* 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++)
@@ -410,7 +396,7 @@ mc_thread_main(int argc, char *argv[])
close(att_sub);
- close(state_sub);
+ close(control_mode_sub);
close(manual_sub);
close(actuator_pub);
close(att_sp_pub);
@@ -467,11 +453,11 @@ int multirotor_att_control_main(int argc, char *argv[])
thread_should_exit = false;
mc_task = task_spawn_cmd("multirotor_att_control",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 15,
- 2048,
- mc_thread_main,
- NULL);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 15,
+ 2048,
+ mc_thread_main,
+ NULL);
exit(0);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-30 06:12:31 +0000