zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Wip on inner rate loop

sbg
daregger 13 years ago
parent
0b26ca84d4
commit
b50bc7798a
4 changed files with 37 additions and 20 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      .gitignore
  2. 9
      apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
  3. 6
      apps/commander/state_machine_helper.c
  4. 41
      apps/multirotor_att_control/multirotor_rate_control.c

1
.gitignore vendored
Unescape View File

@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h
Make.dep Make.dep
*.o *.o
*.a *.a
*~
Images/*.bin Images/*.bin
Images/*.px4 Images/*.px4
nuttx/Make.defs nuttx/Make.defs

9
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
Unescape View File

@ -293,12 +293,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[0] += raw.gyro_rad_s[0];
gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[1] += raw.gyro_rad_s[1];
gyro_offsets[2] += raw.gyro_rad_s[2]; gyro_offsets[2] += raw.gyro_rad_s[2];
offset_count+=1;
if (hrt_absolute_time() - start_time > 3000000LL) { if (hrt_absolute_time() - start_time > 3000000LL) {
initialized = true; initialized = true;
gyro_offsets[0] /= offset_count; gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count; gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count; gyro_offsets[2] /= offset_count;
printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) );
} }
} else { } else {
@ -315,9 +316,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_last_timestamp[0] = raw.timestamp; sensor_last_timestamp[0] = raw.timestamp;
} }
z_k[0] = raw.gyro_rad_s[0]; z_k[0] = raw.gyro_rad_s[0]-gyro_offsets[0];
z_k[1] = raw.gyro_rad_s[1]; z_k[1] = raw.gyro_rad_s[1]-gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2]; z_k[2] = raw.gyro_rad_s[2]-gyro_offsets[2];
/* update accelerometer measurements */ /* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) { if (sensor_last_count[1] != raw.accelerometer_counter) {

6
apps/commander/state_machine_helper.c
Unescape View File

@ -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; current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */ /* enable attitude control per default */
current_status->flag_control_attitude_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 (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) { 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->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true; current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_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 (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) { 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->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true; current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_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 (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) { if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {

41
apps/multirotor_att_control/multirotor_rate_control.c
Unescape View File

@ -1,8 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Tobias Naegeli <naegelit@student.ethz.ch> * Author: Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch> * Lorenz Meier <lm@inf.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -35,7 +35,11 @@
/** /**
* @file multirotor_rate_control.c * @file multirotor_rate_control.c
*
* Implementation of rate controller * Implementation of rate controller
*
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/ */
#include "multirotor_rate_control.h" #include "multirotor_rate_control.h"
@ -50,10 +54,11 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <arch/board/up_hrt.h> #include <arch/board/up_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.f); 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_P, 40.0f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); 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, 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 roll_control_last = 0;
static float pitch_control_last=0; static float pitch_control_last = 0;
static uint64_t last_run = 0; static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time(); 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 */ /* calculate current control outputs */
/* control pitch (forward) output */ /* 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); 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; /* increase resilience to faulty control inputs */
/* control roll (left/right) output */ 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); 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 */ /* 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[0] = roll_control;
actuators->control[1] = pitch_control; actuators->control[1] = pitch_control;

Write Preview
Loading…
Cancel
Save