From 2596f4351109bfa04068f84b4016e4d33d1e8743 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Mon, 23 Aug 2010 02:35:39 +0000 Subject: [PATCH] OP-120 AHRS: Moved conversion of quaternion to RPY over from AHRS to OP to minimize transfer and computational load on AHRS. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1381 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs.c | 34 +++++-------------- .../OpenPilot/Modules/AHRSComms/ahrs_comms.c | 12 ++++--- flight/PiOS/inc/pios_opahrs_proto.h | 5 --- 3 files changed, 16 insertions(+), 35 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 383ffbc78..cc717cd60 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -78,8 +78,8 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler"))) #define ACCEL_SCALE ( (VDD / FULL_RANGE) / ACCEL_SENSITIVITY * 2 / ACCEL_RANGE * ACCEL_GRAVITY ) #define ACCEL_OFFSET -2048 -#define GYRO_SENSITIVITY ( 2.0 / 1000 ) /* V sec deg^-1 */ -#define RAD_PER_DEGREE ( 3.14159 / 180 ) +#define GYRO_SENSITIVITY ( 2.0 / 1000 ) /* 2 mV / (deg s^-1) */ +#define RAD_PER_DEGREE ( M_PI / 180 ) #define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE ) #define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */ /** @@ -134,12 +134,6 @@ struct attitude_solution { float q3; float q4; } quaternion; - - struct { - float roll; - float pitch; - float yaw; - } euler; }; struct altitude_sensor { @@ -317,7 +311,6 @@ int main() if(ahrs_algorithm == INSGPS_Algo) { /******************** INS ALGORITHM **************************/ - float rpy[3]; // format data for INS algo gyro[0] = gyro_data.filtered.x; @@ -348,15 +341,10 @@ int main() else MagCorrection(mag); - Quaternion2RPY(Nav.q,rpy); attitude_data.quaternion.q1 = Nav.q[0]; attitude_data.quaternion.q2 = Nav.q[1]; attitude_data.quaternion.q3 = Nav.q[2]; attitude_data.quaternion.q4 = Nav.q[3]; - attitude_data.euler.roll = rpy[0]; - attitude_data.euler.pitch = rpy[1]; - attitude_data.euler.yaw = rpy[2]; - if(attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360; } else if( ahrs_algorithm == SIMPLE_Algo ) { @@ -364,10 +352,9 @@ int main() float rpy[3]; /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /* Very simple computation of the heading and attitude from accel. */ - rpy[2] = attitude_data.euler.yaw = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI; - rpy[1] = attitude_data.euler.pitch = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI; - rpy[0] = attitude_data.euler.roll = -atan2(accel_data.filtered.y,accel_data.filtered.z) * 180 / M_PI; - if (attitude_data.euler.yaw < 0) attitude_data.euler.yaw += 360.0; + rpy[2] = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI; + rpy[1] = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI; + rpy[0] = -atan2(accel_data.filtered.y,accel_data.filtered.z) * 180 / M_PI; RPY2Quaternion(rpy,q); attitude_data.quaternion.q1 = q[0]; @@ -557,8 +544,8 @@ void converge_insgps() // This should be done directly but I'm too dumb. float rpy[3]; Quaternion2RPY(Nav.q, rpy); - rpy[1] = attitude_data.euler.pitch = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI; - rpy[0] = attitude_data.euler.roll = -atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI; + rpy[1] = -atan2(accel_data.filtered.x, accel_data.filtered.z) * 180 / M_PI; + rpy[0] = -atan2(accel_data.filtered.y, accel_data.filtered.z) * 180 / M_PI; // Get magnetic readings PIOS_HMC5843_ReadMag(mag_data.raw.axis); mag[0] = -mag_data.raw.axis[1]; @@ -768,9 +755,6 @@ void process_spi_request(void) user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2; user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3; user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4; - user_tx_v1.payload.user.v.rsp.update.euler.roll = attitude_data.euler.roll; - user_tx_v1.payload.user.v.rsp.update.euler.pitch = attitude_data.euler.pitch; - user_tx_v1.payload.user.v.rsp.update.euler.yaw = attitude_data.euler.yaw; // TODO: separate this from INSGPS user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0]; @@ -783,9 +767,7 @@ void process_spi_request(void) user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2; user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3; user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4; - user_tx_v1.payload.user.v.rsp.update.euler.roll = attitude_data.euler.roll; - user_tx_v1.payload.user.v.rsp.update.euler.pitch = attitude_data.euler.pitch; - user_tx_v1.payload.user.v.rsp.update.euler.yaw = attitude_data.euler.yaw; + dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); lfsm_user_set_tx_v1 (&user_tx_v1); break; diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index 4f114caf0..c9839bf8d 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c @@ -396,10 +396,14 @@ static void process_update(struct opahrs_msg_v1_rsp_update * update) data.q3 = update->quaternion.q3; data.q4 = update->quaternion.q4; - data.Roll = update->euler.roll; - data.Pitch = update->euler.pitch; - data.Yaw = update->euler.yaw; - + float q[4] = {data.q1, data.q2, data.q3, data.q4}; + float rpy[3]; + Quaternion2RPY(q,rpy); + data.Roll = rpy[0]; + data.Pitch = rpy[1]; + data.Yaw = rpy[2]; + if(data.Yaw < 0) data.Yaw += 360; + AttitudeActualSet(&data); /* diff --git a/flight/PiOS/inc/pios_opahrs_proto.h b/flight/PiOS/inc/pios_opahrs_proto.h index 1694e941b..c1a68259b 100644 --- a/flight/PiOS/inc/pios_opahrs_proto.h +++ b/flight/PiOS/inc/pios_opahrs_proto.h @@ -281,11 +281,6 @@ struct opahrs_msg_v1_rsp_update { float q3; float q4; } quaternion; - struct { - float roll; - float pitch; - float yaw; - } euler; float NED[3]; float Vel[3]; } __attribute__((__packed__));