diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 80b392f99c..a088ba1ba6 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1356,6 +1356,8 @@ int commander_thread_main(int argc, char *argv[]) uint64_t failsave_ll_start_time = 0; bool state_changed = true; + bool param_init_forced = true; + while (!thread_should_exit) { @@ -1386,10 +1388,10 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ orb_check(param_changed_sub, &new_data); - if (new_data) { + if (new_data || param_init_forced) { + param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); @@ -1398,7 +1400,6 @@ int commander_thread_main(int argc, char *argv[]) if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } - /* disable manual override for all systems that rely on electronic stabilization */ if (current_status.system_type == MAV_TYPE_QUADROTOR || current_status.system_type == MAV_TYPE_HEXAROTOR || diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index e94d7c55d8..b98736cee9 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -197,7 +197,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* load new parameters with lower rate */ - if (motor_skip_counter % 1000 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); @@ -206,6 +206,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) { + pid_reset_integral(&pitch_controller); + pid_reset_integral(&roll_controller); + } + + /* calculate current control outputs */ /* control pitch (forward) output */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 93f7085aec..edb50a96e8 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -150,16 +150,13 @@ 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) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; 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; - } + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + last_run = hrt_absolute_time(); @@ -175,37 +172,32 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,1000.0f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,1000.0f, PID_MODE_DERIVATIV_CALC); + } /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + + /* load new parameters with lower rate */ + parameters_update(&h, &p); + + /* apply parameters */ + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); } - /* 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"); - } /* 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"); - } + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , rates[0], 0.0f, deltaT); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], 0.0f, deltaT); + /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 135d97bb34..9cdb98e23b 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -84,32 +84,45 @@ mixer_tick(void) int i; bool should_arm; + /* check that we are receiving fresh data from the FMU */ + if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + /* too many frames without FMU input, time to go to failsafe */ + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; + lib_lowprintf("\nRX timeout\n"); + } + /* * Decide which set of inputs we're using. */ - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_OUTPUT_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; + /* this is for planes, where manual override makes sense */ + if(system_state.manual_override_ok) { + /* if everything is ok */ + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { + /* we have recent control data from the FMU */ + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* when override is on or the fmu is not available */ + } else if (system_state.rc_channels > 0) { + control_count = system_state.rc_channels; + control_values = &system_state.rc_channel_data[0]; + } else { + /* we have no control input (no FMU, no RC) */ - /* check that we are receiving fresh data from the FMU */ - if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { - - /* too many frames without FMU input, time to go to failsafe */ - system_state.mixer_manual_override = true; - system_state.mixer_fmu_available = false; - lib_lowprintf("\nRX timeout\n"); + // XXX builtin failsafe would activate here + control_count = 0; } - } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { - /* we have control data from an R/C input */ - control_count = system_state.rc_channels; - control_values = &system_state.rc_channel_data[0]; + /* this is for multicopters, etc. where manual override does not make sense */ } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; + /* if the fmu is available whe are good */ + if(system_state.mixer_fmu_available) { + control_count = PX4IO_OUTPUT_CHANNELS; + control_values = &system_state.fmu_channel_data[0]; + /* we better shut everything off */ + } else { + control_count = 0; + } } /* diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa250..49315cdc9f 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -183,3 +183,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo return pid->last_output; } + + +__EXPORT void pid_reset_integral(PID_t *pid) +{ + pid->integral = 0; +} diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index b51afef9ef..64d6688677 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -72,6 +72,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT void pid_reset_integral(PID_t *pid); #endif /* PID_H_ */