diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:45:24 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:45:24 +0100 |
commit | cded2787f0cd0794a73cf58ea4ecd993c5e304d6 (patch) | |
tree | dc59963441075c1b0543098a93bbbe3f88bc8c11 /apps/multirotor_att_control | |
parent | cf563eda8648534475705b6211bf4040ef9e193f (diff) | |
download | px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.tar.gz px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.tar.bz2 px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.zip |
Fixed code style for multirotor_att_control app
Diffstat (limited to 'apps/multirotor_att_control')
5 files changed, 51 insertions, 25 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 1b03e8c22..d94c0a69c 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* + /* * Do not rate-limit the loop to prevent aliasing * if rate-limiting would be desired later, the line below would * enable it. @@ -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; @@ -232,7 +238,7 @@ mc_thread_main(int argc, char *argv[]) /* 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) { @@ -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; } @@ -257,13 +263,14 @@ mc_thread_main(int argc, char *argv[]) /* * 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; + } 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); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index b98736cee..76dbb36d3 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -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++; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h index abc94d617..2cf83e443 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.h +++ b/apps/multirotor_att_control/multirotor_attitude_control.h @@ -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_ */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 93f7085ae..deba1ac03 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -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; } @@ -186,12 +187,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } /* calculate current control outputs */ - + /* 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; diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h index 3c3c50801..03dec317a 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.h +++ b/apps/multirotor_att_control/multirotor_rate_control.h @@ -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_ */ |