diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-01 14:14:44 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-09-01 14:14:44 +0200 |
commit | 89526c80afd018440574dfd7f22f3016d6bbb96f (patch) | |
tree | ab3fdd460fbbef9de89083eeb0a97ad1aee175ef /src/modules | |
parent | f0a750714ae07976b295c0a857d85cbd57a09a72 (diff) | |
parent | a660dc3fa6dd8c4bee54b834cec6ce1a94a44b33 (diff) | |
download | px4-firmware-89526c80afd018440574dfd7f22f3016d6bbb96f.tar.gz px4-firmware-89526c80afd018440574dfd7f22f3016d6bbb96f.tar.bz2 px4-firmware-89526c80afd018440574dfd7f22f3016d6bbb96f.zip |
Merge branch 'master' of github.com:PX4/Firmware into pid_fix
Diffstat (limited to 'src/modules')
3 files changed, 14 insertions, 3 deletions
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 236052b56..86bda3c75 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt); - // Convert q->R. + // Convert q->R, This R converts inertial frame to body frame. Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11 Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3); // 12 Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2); // 13 @@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets)); /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R, Rot_matrix, sizeof(float)*9); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c index 984bd1329..363961819 100755 --- a/src/modules/position_estimator_mc/position_estimator_mc_main.c +++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c @@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[]) local_pos_est.vz = x_z_aposteriori_k[1]; local_pos_est.timestamp = hrt_absolute_time(); if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){ - orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + if(local_pos_est_pub > 0) + orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est); + else + local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est); + //char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]); + //mavlink_log_info(mavlink_fd, buf); } } } /* end of poll return value check */ diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index a7cda34a8..686fd765c 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + //! For quaternion-based attitude control + float q_d[4]; /** Desired quaternion for quaternion control */ + bool q_d_valid; /**< Set to true if quaternion vector is valid */ + float q_e[4]; /** Attitude error in quaternion */ + bool q_e_valid; /**< Set to true if quaternion error vector is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ }; |