Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 03:27:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

Vectornav as AHRS, minor fix in the float_euler stabilization settings (#2013)

Browse Source
This commit is contained in:
Michal Podhradsky
2017-02-16 14:53:48 -08:00
committed by GitHub
parent 15d00ada9a
commit 861986abae
4 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/stabilization_float_euler.xml
+1 -1
View File
@@ -49,7 +49,7 @@
<dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN" persistent="true"/> <dl_setting var="stabilization_gains.d.y" min="1" step="1" max="4000" shortname="dgain q" param="STABILIZATION_ATTITUDE_THETA_DGAIN" persistent="true"/>
<dl_setting var="stabilization_gains.rates_d.y" min="0" step="1" max="500" shortname="dgaind q" param="STABILIZATION_ATTITUDE_THETA_DGAIN_D" persistent="true"/> <dl_setting var="stabilization_gains.rates_d.y" min="0" step="1" max="500" shortname="dgaind q" param="STABILIZATION_ATTITUDE_THETA_DGAIN_D" persistent="true"/>
<dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN" persistent="true"/> <dl_setting var="stabilization_gains.dd.y" min="0" step="1" max="1000" shortname="ddgain q" param="STABILIZATION_ATTITUDE_THETA_DDGAIN" persistent="true"/>
<dl_setting var="stabilization_gains.p.z" min="1" step="1" max="4000" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN" persistent="true"/> <dl_setting var="stabilization_gains.p.z" min="1" step="1" max="8000" shortname="pgain psi" param="STABILIZATION_ATTITUDE_PSI_PGAIN" persistent="true"/>
<dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN" persistent="true"/> <dl_setting var="stabilization_gains.i.z" min="0" step="1" max="400" shortname="igain psi" param="STABILIZATION_ATTITUDE_PSI_IGAIN" persistent="true"/>
<dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN" persistent="true"/> <dl_setting var="stabilization_gains.d.z" min="1" step="1" max="4000" shortname="dgain r" param="STABILIZATION_ATTITUDE_PSI_DGAIN" persistent="true"/>
<dl_setting var="stabilization_gains.rates_d.z" min="0" step="1" max="500" shortname="dgaind r" param="STABILIZATION_ATTITUDE_PHI_DGAIN_D" persistent="true"/> <dl_setting var="stabilization_gains.rates_d.z" min="0" step="1" max="500" shortname="dgaind r" param="STABILIZATION_ATTITUDE_PHI_DGAIN_D" persistent="true"/>
sw/airborne/subsystems/ahrs/ahrs_vectornav.c
+9 -6
View File
@@ -72,11 +72,17 @@ void ahrs_vectornav_event(void)
if (ahrs_vn.vn_packet.msg_available) { if (ahrs_vn.vn_packet.msg_available) {
vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data)); vn200_read_message(&(ahrs_vn.vn_packet),&(ahrs_vn.vn_data));
ahrs_vn.vn_packet.msg_available = false; ahrs_vn.vn_packet.msg_available = false;
ahrs_vectornav_propagate();
if (ahrs_vectornav_is_enabled()) {
ahrs_vectornav_propagate();
}
// send ABI messages // send ABI messages
uint32_t now_ts = get_sys_time_usec(); uint32_t now_ts = get_sys_time_usec();
// in fixed point for sending as ABI and telemetry msgs
RATES_BFP_OF_REAL(ahrs_vn.gyro_i, ahrs_vn.vn_data.gyro);
AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i); AbiSendMsgIMU_GYRO_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.gyro_i);
ACCELS_BFP_OF_REAL(ahrs_vn.accel_i, ahrs_vn.vn_data.accel);
AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i); AbiSendMsgIMU_ACCEL_INT32(IMU_VECTORNAV_ID, now_ts, &ahrs_vn.accel_i);
} }
} }
@@ -124,11 +130,8 @@ void ahrs_vectornav_propagate(void)
// Attitude [deg] // Attitude [deg]
static struct FloatQuat imu_quat; // convert from euler to quat static struct FloatQuat imu_quat; // convert from euler to quat
float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude); float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude);
static struct FloatRMat imu_rmat; // convert from quat to rmat stateSetNedToBodyQuat_f(&imu_quat);
float_rmat_of_quat(&imu_rmat, &imu_quat);
static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ahrs_vn.body_to_imu));
stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]
} }
sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.c
+4
View File
@@ -59,6 +59,10 @@ static bool ahrs_vectornav_enable_output(bool enable)
return ahrs_vectornav_output_enabled; return ahrs_vectornav_output_enabled;
} }
bool ahrs_vectornav_is_enabled(void){
return ahrs_vectornav_output_enabled;
}
void ahrs_vectornav_register(void) void ahrs_vectornav_register(void)
{ {
ahrs_vectornav_output_enabled = AHRS_VECTORNAV_OUTPUT_ENABLED; ahrs_vectornav_output_enabled = AHRS_VECTORNAV_OUTPUT_ENABLED;
sw/airborne/subsystems/ahrs/ahrs_vectornav_wrapper.h
+1
View File
@@ -35,5 +35,6 @@
#endif #endif
extern void ahrs_vectornav_register(void); extern void ahrs_vectornav_register(void);
extern bool ahrs_vectornav_is_enabled(void);
#endif /* AHRS_VECTORNAV_WRAPPER_H */ #endif /* AHRS_VECTORNAV_WRAPPER_H */