From b50bc7798ac463de3e0c3147df46a3f7227df8c3 Mon Sep 17 00:00:00 2001 From: daregger Date: Tue, 16 Oct 2012 16:49:45 +0200 Subject: Wip on inner rate loop --- .gitignore | 1 + .../attitude_estimator_ekf_main.c | 9 ++--- apps/commander/state_machine_helper.c | 6 ++-- .../multirotor_rate_control.c | 41 +++++++++++++++------- 4 files changed, 37 insertions(+), 20 deletions(-) mode change 100755 => 100644 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100755 => 100644 apps/multirotor_att_control/multirotor_rate_control.c diff --git a/.gitignore b/.gitignore index 38cb13162..ea416fae4 100644 --- a/.gitignore +++ b/.gitignore @@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h Make.dep *.o *.a +*~ Images/*.bin Images/*.px4 nuttx/Make.defs diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100755 new mode 100644 index b507b4c10..a291a4914 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -293,12 +293,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[2] += raw.gyro_rad_s[2]; - + offset_count+=1; if (hrt_absolute_time() - start_time > 3000000LL) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; + printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) ); } } else { @@ -315,9 +316,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_last_timestamp[0] = raw.timestamp; } - z_k[0] = raw.gyro_rad_s[0]; - z_k[1] = raw.gyro_rad_s[1]; - z_k[2] = raw.gyro_rad_s[2]; + z_k[0] = raw.gyro_rad_s[0]-gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1]-gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2]-gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 7d766bcdb..b21f3858f 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -504,7 +504,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* enable attitude control per default */ current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -519,7 +519,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_ current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED; current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { @@ -534,7 +534,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; current_status->flag_control_manual_enabled = true; current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = false; // XXX + current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c old mode 100755 new mode 100644 index 2809c4533..c132665d2 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -1,8 +1,8 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Tobias Naegeli - * @author Lorenz Meier + * Author: Tobias Naegeli + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,11 @@ /** * @file multirotor_rate_control.c + * * Implementation of rate controller + * + * @author Tobias Naegeli + * @author Lorenz Meier */ #include "multirotor_rate_control.h" @@ -50,10 +54,11 @@ #include #include - PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ - PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); - PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 40.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 40.0f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); @@ -144,8 +149,8 @@ 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 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; last_run = hrt_absolute_time(); @@ -174,15 +179,25 @@ 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 * deltaT *((rate_sp->pitch)*p.attrate_lim-rates[1])-p.attrate_d*(pitch_control_last); - pitch_control_last=pitch_control; - /* control roll (left/right) output */ + /* increase resilience to faulty control inputs */ + if (isfinite(pitch_control)) { + pitch_control_last = pitch_control; + } else { + pitch_control = 0.0f; + } + /* control roll (left/right) output */ float roll_control = p.attrate_p * deltaT * ((rate_sp->roll)*p.attrate_lim-rates[0])-p.attrate_d*(roll_control_last); - roll_control_last=roll_control; + /* increase resilience to faulty control inputs */ + if (isfinite(roll_control)) { + roll_control_last = roll_control; + } else { + roll_control = 0.0f; + } + /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2] ); + float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.attrate_lim-rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; -- cgit v1.2.3