From 5d3d17d025fa860879b145a99ec80afb7db38edc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 10:38:23 +0200 Subject: Increased priority of MAVLink receiver thread --- apps/mavlink/mavlink.c | 2 +- apps/mavlink/mavlink_receiver.c | 5 +++++ apps/multirotor_att_control/multirotor_attitude_control.c | 2 +- 3 files changed, 7 insertions(+), 2 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e893ea951..075dc9e3b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1,4 +1,4 @@ -/**************************************************************************** + /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Lorenz Meier diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index ad46c3ede..060bff2b4 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -487,6 +487,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, ¶m); + pthread_attr_setstacksize(&receiveloop_attr, 2048); pthread_t thread; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 00b759d73..33cab5cc9 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -248,7 +248,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s att->roll, att->rollspeed, deltaT)*1/10.0f; /* 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),cos(att->yaw - att_sp->yaw_body)); -- cgit v1.2.3 From 23d294453bbdade03a67002f62b898e7aca65a70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:08:33 +0200 Subject: Fixed a range of initialization issues in filter, does not any more emit NaN in first iteration --- .../attitude_estimator_ekf_main.c | 59 ++++++++++------------ .../attitude_estimator_ekf_params.c | 6 +-- .../codegen/attitudeKalmanfilter.c | 12 ++--- 3 files changed, 36 insertions(+), 41 deletions(-) (limited to 'apps') diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index a291a4914..7bb8490e5 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -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++; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index f20d1b204..4fc2e0452 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -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); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 885c01bf3..04f6ea267 100644 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -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]); } -- cgit v1.2.3 From 8b000b331704b2d37a8c47ae1bc480b784965db4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:09:28 +0200 Subject: Fixed an abort condition, fixed value initialization, implemented naive three-step calibration --- apps/commander/commander.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 43df6ceb2..746c0e791 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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); @@ -333,13 +339,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) 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(); @@ -823,7 +830,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, ¤t_status); + ioctl(buzzer, TONE_SET_ALARM, 2); mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; -- cgit v1.2.3 From d4e6a9d7a1e448a5cc55436c0bb5f951694bc46e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:10:04 +0200 Subject: Minor code style fixes, removed dead code --- apps/mavlink/mavlink_receiver.c | 13 ++----------- apps/mavlink/orb_listener.c | 17 ++++------------- 2 files changed, 6 insertions(+), 24 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 060bff2b4..15ed70dbd 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -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; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 39358922d..63a39e4ee 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -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++; } @@ -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); -- cgit v1.2.3 From e4645c0a411b62ebe58c14639f5259e6a444178d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:10:32 +0200 Subject: Initialized all sensor fields to zero --- apps/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'apps') diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index b84b58406..2ab1a9203 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -993,6 +993,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; -- cgit v1.2.3 From 2d631fb0059cecdc429c5847e8dbede82348d129 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:26:56 +0200 Subject: Various fixes to attitude control, flyable, needs parameter tuning --- apps/multirotor_att_control/multirotor_att_control_main.c | 2 +- apps/multirotor_att_control/multirotor_attitude_control.c | 11 ++++++----- apps/multirotor_att_control/multirotor_rate_control.c | 14 ++++++++++++-- 3 files changed, 19 insertions(+), 8 deletions(-) (limited to 'apps') diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index cb82036fe..82d4c0f25 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -195,7 +195,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; + rates_sp.yaw = att.yaw + manual.yaw * 2.0f; rates_sp.thrust = manual.throttle; //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 33cab5cc9..c3da85c5c 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -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)); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index c132665d2..db3f1a0a6 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -153,6 +153,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,7 +180,8 @@ 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); + printf("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 */ @@ -185,6 +193,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pitch_control_last = pitch_control; } else { pitch_control = 0.0f; + printf("rej. NaN ctrl pitch\n"); } /* control roll (left/right) output */ @@ -194,10 +203,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, roll_control_last = roll_control; } else { roll_control = 0.0f; + printf("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)*p.yawrate_lim-rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; -- cgit v1.2.3 From 6a48b91beae575ad4684151f08c75b079caffe5c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:27:21 +0200 Subject: Lowering default rates at 57600 --- apps/mavlink/mavlink.c | 6 +++--- apps/mavlink/orb_listener.c | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'apps') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 075dc9e3b..293bbe00c 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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 */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 63a39e4ee..c2428874f 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -551,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)); -- cgit v1.2.3 From d1429f266da33aa3355b228adffe56f74a9c3cd1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 18:27:49 +0200 Subject: Calibration progress, needs sphere fitting --- apps/commander/commander.c | 44 +++++++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 21 deletions(-) (limited to 'apps') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 746c0e791..687b02d4f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -325,8 +325,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) { @@ -334,15 +334,17 @@ 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)) { @@ -362,26 +364,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[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[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]; + } calibration_counter++; } else { -- cgit v1.2.3 From dff0051568602d60f6b8b3d501a2ea78e98b9d7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 19:02:57 +0200 Subject: Map inputs to the controller we actually want --- apps/multirotor_att_control/multirotor_att_control_main.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'apps') diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 82d4c0f25..dfb778fd0 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -195,7 +195,7 @@ mc_thread_main(int argc, char *argv[]) rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; - rates_sp.yaw = att.yaw + manual.yaw * 2.0f; + rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; //printf("rates\n"); rates_sp.timestamp = hrt_absolute_time(); @@ -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(); } -- cgit v1.2.3 From c70c626915fc70d87088c6ae66152ed536cacc96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Oct 2012 13:40:17 +0200 Subject: Removed dead code --- apps/ardrone_interface/ardrone_interface.c | 1 - 1 file changed, 1 deletion(-) (limited to 'apps') diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d4a473b6..833425aa6 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -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); -- cgit v1.2.3 From 5ec5754f26f1059847b7f149bcbdf2b0d11a5115 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Oct 2012 17:34:06 +0200 Subject: brought controller back to last tuned state --- apps/multirotor_att_control/multirotor_rate_control.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) (limited to 'apps') diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index db3f1a0a6..7401ef0f6 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -52,6 +52,7 @@ #include #include #include +#include #include PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 20.0f); /* same on Flamewheel */ @@ -180,34 +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("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz\n", + 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; - printf("rej. NaN ctrl pitch\n"); + 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; - printf("rej. NaN ctrl roll\n"); + warnx("rej. NaN ctrl roll\n"); } /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * ((rate_sp->yaw)*p.yawrate_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; -- cgit v1.2.3