Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[nps] update crrcsim and cleanup accel

Browse Source
This commit is contained in:
Felix Ruess
2015-03-10 14:43:06 +01:00
parent d56a4a5381
commit 478f95bc10
3 changed files with 24 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile
-2
View File
@@ -23,8 +23,6 @@ nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy $(shell pcre-co
nps.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps
nps.LDFLAGS += $(shell sdl-config --libs)
nps.CFLAGS += -DNPS_ACCEL_FROM_UVWDOT
#
# add the simulator directory to the make searchpath
#
sw/simulator/nps/nps_fdm_crrcsim.c
+19 -9
View File
@@ -430,15 +430,25 @@ static void decode_ahrspacket(struct NpsFdm * fdm, byte* buffer)
void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
{
/* acceleration (0.1670132517315938e04 m/s^2 to m/s^2) */
fdm->body_ecef_accel.x = (double)ShortOfBuf(buffer,3)*5.98755e-04;
fdm->body_ecef_accel.y = (double)ShortOfBuf(buffer,5)*5.98755e-04;
fdm->body_ecef_accel.z = (double)ShortOfBuf(buffer,7)*5.98755e-04;
fdm->body_accel.x = (double)ShortOfBuf(buffer,3)*5.98755e-04;
fdm->body_accel.y = (double)ShortOfBuf(buffer,5)*5.98755e-04;
fdm->body_accel.z = (double)ShortOfBuf(buffer,7)*5.98755e-04;
/* since we don't get acceleration in ecef frame, use ECI for now */
fdm->body_ecef_accel.x = fdm->body_accel.x;
fdm->body_ecef_accel.y = fdm->body_accel.y;
fdm->body_ecef_accel.z = fdm->body_accel.z;
/* angular rate (0.9387340515702713e4 rad/s to rad/s) */
fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04;
fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04;
fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04;
fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04;
fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04;
fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04;
/* since we don't get angular velocity in ECEF frame, use the rotvel in ECI frame for now */
fdm->body_ecef_rotvel.p = fdm->body_inertial_rotvel.p;
fdm->body_ecef_rotvel.q = fdm->body_inertial_rotvel.q;
fdm->body_ecef_rotvel.r = fdm->body_inertial_rotvel.r;
/* magnetic field in Gauss */
//fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05;
@@ -451,9 +461,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
#if NPS_CRRCSIM_DEBUG
printf("decode imu | accel %f %f %f | gyro %f %f %f\n",
fdm->body_ecef_accel.x,
fdm->body_ecef_accel.y,
fdm->body_ecef_accel.z,
fdm->body_accel.x,
fdm->body_accel.y,
fdm->body_accel.z,
fdm->body_inertial_rotvel.p,
fdm->body_inertial_rotvel.q,
fdm->body_inertial_rotvel.r);
sw/simulator/nps/nps_sensor_accel.c
+5 -24
View File
@@ -7,7 +7,8 @@
#include NPS_SENSORS_PARAMS
#include "math/pprz_algebra_int.h"
void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time)
{
FLOAT_VECT3_ZERO(accel->value);
accel->min = NPS_ACCEL_MIN;
accel->max = NPS_ACCEL_MAX;
@@ -23,34 +24,14 @@ void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
accel->data_available = FALSE;
}
//#include <stdio.h>
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) {
void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu)
{
if (time < accel->next_update)
return;
#if NPS_ACCEL_FROM_UVWDOT
/* transform gravity to body frame */
struct DoubleVect3 g_body;
double_quat_vmult(&g_body, &fdm.ltp_to_body_quat, &fdm.ltp_g);
// printf(" g_body % .3f % .3f % .3f\n", g_body.x, g_body.y, g_body.z);
// printf(" accel_fdm % .3f % .3f % .3f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
/* substract gravity to acceleration in body frame */
struct DoubleVect3 accelero_body;
VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
#else
struct DoubleVect3 accelero_body;
VECT3_COPY(accelero_body, fdm.body_accel);
#endif
// printf(" accel body % .3f %.3f % .3f\n", accelero_body.x, accelero_body.y, accelero_body.z);
/* transform to imu frame */
struct DoubleVect3 accelero_imu;
MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body);
MAT33_VECT3_MUL(accelero_imu, *body_to_imu, fdm.body_accel);
/* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);