fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 14:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-19 00:39:06 +02:00
parent 97726fa679 5ec5754f26
commit d3ae83cb22
26 changed files with 1227 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
-1
View File
@@ -286,7 +286,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* close uarts */
close(ardrone_write);
//ar_multiplexing_deinit(gpios);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+27 -32
View File
@@ -71,11 +71,10 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 1.0f;
static float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9]; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< */
static float x_aposteriori[12];
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< states */
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
@@ -88,21 +87,10 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
};
}; /**< init: diagonal matrix with big values */
static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float x_aposteriori[12];
static float P_aposteriori[144];
/* output euler angles */
static float euler[3] = {0.0f, 0.0f, 0.0f};
@@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
/* advertise debug value */
struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
orb_advert_t pub_dbg = -1;
/* keep track of sensor updates */
uint32_t sensor_last_count[3] = {0, 0, 0};
@@ -293,13 +281,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;
offset_count++;
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 {
@@ -316,9 +304,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]-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];
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) {
@@ -362,7 +350,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
static bool const_initialized = false;
/* initialize with good values once we have a reasonable dt estimate */
if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/)
if (!const_initialized && dt < 0.05 && dt > 0.005)
{
dt = 0.005f;
parameters_update(&ekf_param_handles, &ekf_params);
@@ -388,16 +376,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
continue;
}
dt = 0.004f;
uint64_t timing_start = hrt_absolute_time();
// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
// Rot_matrix, x_aposteriori, P_aposteriori);
attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
euler, Rot_matrix, x_aposteriori, P_aposteriori);
/* swap values for next iteration */
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
/* swap values for next iteration, check for fatal inputs */
if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
} else {
/* due to inputs or numerical failure the output is invalid, skip it */
continue;
}
uint64_t timing_diff = hrt_absolute_time() - timing_start;
// /* print rotation matrix every 200th time */
@@ -425,7 +416,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
// sprintf(name, "xapo #%d", i);
// memcpy(dbg.key, name, sizeof(dbg.key));
// dbg.value = x_aposteriori[i];
// if (pub_dbg < 0) {
// pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
// } else {
// orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
// }
printcounter++;
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+3 -3
View File
@@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
/* gyro measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
/* accelerometer measurement noise */
PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+6 -6
View File
@@ -50,7 +50,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
b_u1 = -1;
}
y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1);
} else if (u1 == 0.0F) {
if (u0 > 0.0F) {
y = RT_PIF / 2.0F;
@@ -60,7 +60,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
y = 0.0F;
}
} else {
y = (real32_T)atan2(u0, u1);
y = (real32_T)atan2f(u0, u1);
}
return y;
@@ -776,12 +776,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
Rot_matrix[6 + i] = z_n_b[i];
}
/* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */
/* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */
/* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */
/* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */
eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]);
eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
}
apps/commander/commander.c
+34 -23
View File
@@ -292,10 +292,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct sensor_combined_s raw;
/* 30 seconds */
uint64_t calibration_interval = 30 * 1000 * 1000;
uint64_t calibration_interval = 45 * 1000 * 1000;
unsigned int calibration_counter = 0;
float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN};
/*
* FLT_MIN is not the most negative float number,
* but the smallest number by magnitude float can
* represent. Use -FLT_MAX to initialize the most
* negative number
*/
float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
int fd = open(MAG_DEVICE_PATH, 0);
@@ -322,8 +328,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = 0;
const char axislabels[3] = { 'Z', 'X', 'Y'};
int axis_index = -1;
while (hrt_absolute_time() < calibration_deadline) {
@@ -331,18 +337,21 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
/* user guidance */
if (hrt_absolute_time() > axis_deadline &&
if (hrt_absolute_time() >= axis_deadline &&
axis_index < 3) {
axis_index++;
char buf[50];
sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
ioctl(buzzer, TONE_SET_ALARM, 2);
axis_deadline += calibration_interval / 3;
axis_index++;
}
if (!(axis_index < 3)) {
continue;
break;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
@@ -358,26 +367,26 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* get min/max values */
/* ignore other axes */
if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
mag_min[axis_index] = raw.magnetometer_ga[axis_index];
if (raw.magnetometer_ga[0] < mag_min[0]) {
mag_min[0] = raw.magnetometer_ga[0];
}
else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
mag_max[axis_index] = raw.magnetometer_ga[axis_index];
else if (raw.magnetometer_ga[0] > mag_max[0]) {
mag_max[0] = raw.magnetometer_ga[0];
}
// if (raw.magnetometer_ga[1] < mag_min[1]) {
// mag_min[1] = raw.magnetometer_ga[1];
// }
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
// mag_max[1] = raw.magnetometer_ga[1];
// }
if (raw.magnetometer_ga[1] < mag_min[1]) {
mag_min[1] = raw.magnetometer_ga[1];
}
else if (raw.magnetometer_ga[1] > mag_max[1]) {
mag_max[1] = raw.magnetometer_ga[1];
}
// if (raw.magnetometer_ga[2] < mag_min[2]) {
// mag_min[2] = raw.magnetometer_ga[2];
// }
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
// mag_max[2] = raw.magnetometer_ga[2];
// }
if (raw.magnetometer_ga[2] < mag_min[2]) {
mag_min[2] = raw.magnetometer_ga[2];
}
else if (raw.magnetometer_ga[2] > mag_max[2]) {
mag_max[2] = raw.magnetometer_ga[2];
}
calibration_counter++;
} else {
@@ -840,7 +849,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration");
ioctl(buzzer, TONE_SET_ALARM, 2);
do_accel_calibration(status_pub, &current_status);
ioctl(buzzer, TONE_SET_ALARM, 2);
mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
apps/mavlink/mavlink.c
+4 -4
View File
@@ -1,4 +1,4 @@
/****************************************************************************
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
@@ -576,10 +576,10 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
/* 10 Hz / 100 ms ATTITUDE */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
/* 5 Hz / 200 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 5 Hz / 200 ms */
apps/mavlink/mavlink_receiver.c
+7 -11
View File
@@ -225,28 +225,20 @@ handle_message(mavlink_message_t *msg)
uint8_t ml_mode = 0;
bool ml_armed = false;
// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
// ml_armed = true;
// }
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
@@ -259,7 +251,6 @@ handle_message(mavlink_message_t *msg)
offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
//offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ;
if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
ml_armed = false;
@@ -487,6 +478,11 @@ receive_start(int uart)
{
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
struct sched_param param;
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_t thread;
apps/mavlink/orb_listener.c
+6 -15
View File
@@ -141,12 +141,6 @@ l_sensor_combined(struct listener *l)
last_sensor_timestamp = raw.timestamp;
/* send raw imu data */
// mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, raw.accelerometer_raw[0],
// raw.accelerometer_raw[1], raw.accelerometer_raw[2], raw.gyro_raw[0],
// raw.gyro_raw[1], raw.gyro_raw[2], raw.magnetometer_raw[0],
// raw.magnetometer_raw[1], raw.magnetometer_raw[2]);
/* mark individual fields as changed */
uint16_t fields_updated = 0;
static unsigned accel_counter = 0;
@@ -161,19 +155,19 @@ l_sensor_combined(struct listener *l)
}
if (gyro_counter != raw.gyro_counter) {
/* mark first three dimensions as changed */
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
gyro_counter = raw.gyro_counter;
}
if (mag_counter != raw.magnetometer_counter) {
/* mark first three dimensions as changed */
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
mag_counter = raw.magnetometer_counter;
}
if (baro_counter != raw.baro_counter) {
/* mark first three dimensions as changed */
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
baro_counter = raw.baro_counter;
}
@@ -187,8 +181,6 @@ l_sensor_combined(struct listener *l)
raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
/* send pressure */
//mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, raw.timestamp / 1000, raw.baro_pres_mbar, raw.baro_alt_meter, raw.baro_temp_celcius * 100);
sensors_raw_counter++;
}
@@ -559,12 +551,12 @@ uorb_receive_start(void)
/* --- SENSORS RAW VALUE --- */
mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
/* rate limit set externally based on interface speed, set a basic default here */
orb_set_interval(mavlink_subs.sensor_sub, 100); /* 10Hz updates */
orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */
/* --- ATTITUDE VALUE --- */
mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* rate limit set externally based on interface speed, set a basic default here */
orb_set_interval(mavlink_subs.att_sub, 100); /* 10Hz updates */
orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */
/* --- GPS VALUE --- */
mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -631,8 +623,7 @@ uorb_receive_start(void)
pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 8000 bytes */
/* XXX verify, should not need anything like this much unless MAVLink really sucks */
pthread_attr_setstacksize(&uorb_attr, 8192);
pthread_attr_setstacksize(&uorb_attr, 4096);
pthread_t thread;
pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
apps/multirotor_att_control/multirotor_att_control_main.c
+1 -3
View File
@@ -204,9 +204,7 @@ mc_thread_main(int argc, char *argv[])
if (state.flag_control_attitude_enabled) {
att_sp.roll_body = manual.roll;
att_sp.pitch_body = manual.pitch;
att_sp.yaw_body = manual.yaw; // XXX Hack, clean up
/* set yaw rate */
rates_sp.yaw = manual.yaw;
att_sp.yaw_body = att.yaw + manual.yaw * -2.0f;
att_sp.thrust = manual.throttle;
att_sp.timestamp = hrt_absolute_time();
}
apps/multirotor_att_control/multirotor_attitude_control.c
+6 -5
View File
@@ -235,20 +235,21 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
// pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, p.yawrate_awu);
pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu);
printf("delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
}
/* calculate current control outputs */
/* control pitch (forward) output */
rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff,
att->pitch, att->pitchspeed, deltaT)*1/10.0f;
att->pitch, att->pitchspeed, deltaT);
/* control roll (left/right) output */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
att->roll, att->rollspeed, deltaT)*1/10.0f;
att->roll, att->rollspeed, deltaT);
/* control yaw rate */
//float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT)*1/10.0f;
rates_sp->yaw= deltaT*p.yaw_p*atan2f(sinf(att->yaw-att_sp->yaw_body),cos(att->yaw-att_sp->yaw_body));
rates_sp->yaw= deltaT * p.yaw_p * atan2f(sinf(att->yaw - att_sp->yaw_body), cosf(att->yaw - att_sp->yaw_body));
apps/multirotor_att_control/multirotor_rate_control.c
+17 -4
View File
@@ -52,6 +52,7 @@
#include <math.h>
#include <systemlib/pid/pid.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <arch/board/up_hrt.h>
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */
@@ -153,6 +154,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
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;
}
last_run = hrt_absolute_time();
static int motor_skip_counter = 0;
@@ -173,31 +181,36 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz\n",
(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
}
/* 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);
// XXX fix this line so that deltaT is also applied to the D term
float pitch_control = p.attrate_p * deltaT * (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\n");
}
/* 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);
// XXX fix this line so that deltaT is also applied to the D term
float roll_control = p.attrate_p * deltaT * (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\n");
}
/* 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)-rates[2]);
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
apps/sensors/sensors.cpp
+1
View File
@@ -1004,6 +1004,7 @@ Sensors::task_main()
* do advertisements
*/
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
raw.timestamp = hrt_absolute_time();
raw.battery_voltage_v = BAT_VOL_INITIAL;
raw.adc_voltage_v[0] = 0.9f;