diff options
3 files changed, 106 insertions, 49 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 5bc0d5fa3..21c720096 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -56,6 +56,7 @@ #include <sys/prctl.h> #include <arch/board/up_hrt.h> #include <uORB/uORB.h> +#include <drivers/drv_gyro.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> @@ -72,16 +73,66 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static enum { - CONTROL_MODE_RATES = 0, - CONTROL_MODE_ATTITUDE = 1, -} control_mode; - - static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; +static struct actuator_controls_s actuators; +static orb_advert_t actuator_pub; + +/** + * Perform rate control right after gyro reading + */ +static void *rate_control_thread_main(void *arg) +{ + prctl(PR_SET_NAME, "mc rate control", getpid()); + + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + + struct pollfd fds = { .fd = gyro_sub, .events = POLLIN }; + + struct gyro_report gyro_report; + struct vehicle_rates_setpoint_s rates_sp; + memset(&rates_sp, 0, sizeof(rates_sp)); + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + while (!thread_should_exit) { + /* rate control at maximum rate */ + /* wait for a sensor update, check for exit condition every 1000 ms */ + int ret = poll(&fds, 1, 1000); + + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); + } else { + /* get data */ + orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); + 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); + } + + /* perform local lowpass */ + + /* apply controller */ + // if (state.flag_control_rates_enabled) { + /* lowpass gyros */ + // XXX + gyro_lp[0] = gyro_report.x; + gyro_lp[1] = gyro_report.y; + gyro_lp[2] = gyro_report.z; + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + // } + } + } + + return NULL; +} static int mc_thread_main(int argc, char *argv[]) @@ -102,8 +153,6 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - struct actuator_controls_s actuators; - /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -127,8 +176,9 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + 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); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -136,6 +186,13 @@ mc_thread_main(int argc, char *argv[]) /* welcome user */ printf("[multirotor_att_control] starting\n"); + /* ready, spawn pthread */ + pthread_attr_t rate_control_attr; + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -170,16 +227,21 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller /* set yaw rate */ rates_sp.yaw = manual.yaw; + att_sp.thrust = manual.throttle; 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); if (motor_test_mode) { att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - } else { - att_sp.thrust = manual.throttle; + 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_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -187,38 +249,32 @@ 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; + 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; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = 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); } /* decide wether we want rate or position input */ } - /** STEP 2: Identify the controller setup to run and set up the inputs correctly */ + /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ /* run attitude controller */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &actuators); - } - - /* XXX could be also run in an independent loop */ - if (state.flag_control_rates_enabled) { - /* lowpass gyros */ - // XXX - static float gyro_lp[3] = {0, 0, 0}; - gyro_lp[0] = raw.gyro_rad_s[0]; - gyro_lp[1] = raw.gyro_rad_s[1]; - gyro_lp[2] = raw.gyro_rad_s[2]; - - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, NULL, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } - /* STEP 3: publish the result to the vehicle actuators */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } @@ -240,6 +296,8 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); + pthread_join(rate_control_thread, NULL); + fflush(stdout); exit(0); } @@ -258,22 +316,10 @@ usage(const char *reason) int multirotor_att_control_main(int argc, char *argv[]) { int ch; - - control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { - case 'm': - if (!strcmp(optarg, "rates")) { - control_mode = CONTROL_MODE_RATES; - } else if (!strcmp(optarg, "attitude")) { - control_mode = CONTROL_MODE_RATES; - } else { - usage("unrecognized -m value"); - } - optioncount += 2; - break; case 't': motor_test_mode = true; optioncount += 1; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 6e2a54438..2129915d1 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -188,7 +188,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 actuator_controls_s *actuators) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s roll_controller.saturated = 1; } - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + if (actuators) { + actuators->control[0] = roll_control; + actuators->control[1] = pitch_control; + actuators->control[2] = yaw_rate_control; + actuators->control[3] = motor_thrust; + } + + // XXX change yaw rate to yaw pos controller + if (rates_sp) { + rates_sp->roll = roll_control; + rates_sp->pitch = pitch_control; + rates_sp->yaw = yaw_rate_control; + rates_sp->thrust = motor_thrust; + } motor_skip_counter++; } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index aa8d27369..d9d3c0444 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -48,9 +48,10 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.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, - struct actuator_controls_s *actuators); +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, struct actuator_controls_s *actuators); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ |