rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

ahrs: whitespace adjustments

Browse Source 
No functional changes.  Whitespace/tab fixups only.
Use TRUE/FALSE instead of 1/0.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1430 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
stac 2010-08-27 02:14:58 +00:00 committed by stac
parent 670452307d
commit 47df1faa8d
3 changed files with 83 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
flight/AHRS/ahrs.c
View File

@ -680,54 +680,54 @@ void process_spi_request(void)
lfsm_user_set_tx_v1 (&user_tx_v1);
break;
case OPAHRS_MSG_V1_REQ_CALIBRATION:
if(user_rx_v1.payload.user.v.req.calibration.measure_var)
if(user_rx_v1.payload.user.v.req.calibration.measure_var) {
calibration_pending = TRUE;
else {
} else {
accel_var[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0];
accel_var[1] = user_rx_v1.payload.user.v.req.calibration.accel_var[1];
accel_var[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
gyro_bias[0] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[0];
gyro_bias[1] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[1];
gyro_bias[2] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[2];
gyro_var[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
gyro_var[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
gyro_var[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
mag_var[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
mag_var[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
mag_var[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
gyro_var[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
gyro_var[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
gyro_var[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
mag_var[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
mag_var[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
mag_var[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
INSSetAccelVar(accel_var);
float gyro_bias_ins[3] = {0,0,0};
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
INSSetGyroVar(gyro_var);
INSSetMagVar(mag_var);
}
accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
accel_bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
accel_bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
accel_bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
accel_scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
accel_scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
accel_scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
mag_bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
mag_bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
mag_bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
gyro_scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
gyro_scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
gyro_scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
mag_bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
mag_bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
mag_bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
// echo back the values used
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_CALIBRATION);
opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_CALIBRATION);
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] = accel_var[0];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] = accel_var[1];
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] = accel_var[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] = gyro_bias[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] = gyro_bias[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] = gyro_bias[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_var[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_var[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_var[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_var[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_var[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_var[2];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_var[0];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_var[1];
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_var[2];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_var[0];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_var[1];
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_var[2];
dump_spi_message(PIOS_COM_AUX, "C", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1));
lfsm_user_set_tx_v1 (&user_tx_v1);
@ -770,13 +770,13 @@ void process_spi_request(void)
}
if(user_rx_v1.payload.user.v.req.update.gps.updated)
{
gps_data.updated = true;
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
gps_data.updated = true;
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
gps_data.groundspeed = user_rx_v1.payload.user.v.req.update.gps.groundspeed;
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
}
// send out attitude/position estimate

83
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -101,7 +101,7 @@ static void BaroAltitudeUpdatedCb(UAVObjEvent * ev)
static bool GPSPositionIsUpdatedFlag = false;
static void GPSPositionUpdatedCb(UAVObjEvent * ev)
{
GPSPositionIsUpdatedFlag = true;
GPSPositionIsUpdatedFlag = true;
}
static bool HomeLocationIsUpdatedFlag = false;
@ -140,8 +140,11 @@ int32_t AHRSCommsInitialize(void)
return 0;
}
static uint16_t update_errors = 0, attituderaw_errors = 0,
home_errors = 0, calibration_errors = 0, algorithm_errors = 0;
static uint16_t update_errors = 0;
static uint16_t attituderaw_errors = 0;
static uint16_t home_errors = 0;
static uint16_t calibration_errors = 0;
static uint16_t algorithm_errors = 0;
/**
* Module thread, should not return.
@ -308,31 +311,31 @@ static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration)
AHRSCalibrationData data;
AHRSCalibrationGet(&data);
calibration->measure_var = data.measure_var;
calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2];
calibration->measure_var = data.measure_var;
calibration->accel_bias[0] = data.accel_bias[0];
calibration->accel_bias[1] = data.accel_bias[1];
calibration->accel_bias[2] = data.accel_bias[2];
calibration->accel_scale[0] = data.accel_scale[0];
calibration->accel_scale[1] = data.accel_scale[1];
calibration->accel_scale[2] = data.accel_scale[2];
calibration->accel_var[0] = data.accel_var[0];
calibration->accel_var[1] = data.accel_var[1];
calibration->accel_var[2] = data.accel_var[2];
calibration->gyro_bias[0] = data.gyro_bias[0];
calibration->gyro_bias[1] = data.gyro_bias[1];
calibration->gyro_bias[2] = data.gyro_bias[2];
calibration->gyro_scale[0] = data.gyro_scale[0];
calibration->gyro_scale[1] = data.gyro_scale[1];
calibration->gyro_scale[2] = data.gyro_scale[2];
calibration->gyro_var[0] = data.gyro_var[0];
calibration->gyro_var[1] = data.gyro_var[1];
calibration->gyro_var[2] = data.gyro_var[2];
calibration->mag_bias[0] = data.mag_bias[0];
calibration->mag_bias[1] = data.mag_bias[1];
calibration->mag_bias[2] = data.mag_bias[2];
calibration->mag_var[0] = data.mag_var[0];
calibration->mag_var[1] = data.mag_var[1];
calibration->mag_var[2] = data.mag_var[2];
calibration->accel_var[0] = data.accel_var[0];
calibration->accel_var[1] = data.accel_var[1];
calibration->accel_var[2] = data.accel_var[2];
calibration->gyro_bias[0] = data.gyro_bias[0];
calibration->gyro_bias[1] = data.gyro_bias[1];
calibration->gyro_bias[2] = data.gyro_bias[2];
calibration->gyro_scale[0] = data.gyro_scale[0];
calibration->gyro_scale[1] = data.gyro_scale[1];
calibration->gyro_scale[2] = data.gyro_scale[2];
calibration->gyro_var[0] = data.gyro_var[0];
calibration->gyro_var[1] = data.gyro_var[1];
calibration->gyro_var[2] = data.gyro_var[2];
calibration->mag_bias[0] = data.mag_bias[0];
calibration->mag_bias[1] = data.mag_bias[1];
calibration->mag_bias[2] = data.mag_bias[2];
calibration->mag_var[0] = data.mag_var[0];
calibration->mag_var[1] = data.mag_var[1];
calibration->mag_var[2] = data.mag_var[2];
}
@ -345,12 +348,12 @@ static void update_calibration(struct opahrs_msg_v1_rsp_calibration * calibratio
data.accel_var[0] = calibration->accel_var[0];
data.accel_var[1] = calibration->accel_var[1];
data.accel_var[2] = calibration->accel_var[2];
data.gyro_var[0] = calibration->gyro_var[0];
data.gyro_var[1] = calibration->gyro_var[1];
data.gyro_var[2] = calibration->gyro_var[2];
data.mag_var[0] = calibration->mag_var[0];
data.mag_var[1] = calibration->mag_var[1];
data.mag_var[2] = calibration->mag_var[2];
data.gyro_var[0] = calibration->gyro_var[0];
data.gyro_var[1] = calibration->gyro_var[1];
data.gyro_var[2] = calibration->gyro_var[2];
data.mag_var[0] = calibration->mag_var[0];
data.mag_var[1] = calibration->mag_var[1];
data.mag_var[2] = calibration->mag_var[2];
AHRSCalibrationSet(&data);
}
@ -364,22 +367,18 @@ static void load_magnetic_north(struct opahrs_msg_v1_req_north * mag_north)
mag_north->Be[1] = data.Be[1];
mag_north->Be[2] = data.Be[2];
if(data.Be[0] == 0 && data.Be[1] == 0 && data.Be[2] == 0)
{
if(data.Be[0] == 0 && data.Be[1] == 0 && data.Be[2] == 0) {
// in case nothing has been set go to default to prevent NaN in attitude solution
mag_north->Be[0] = 1;
mag_north->Be[1] = 0;
mag_north->Be[2] = 0;
}
else {
} else {
// normalize for unit length here
float len = sqrt(data.Be[0] * data.Be[0] + data.Be[1] * data.Be[1] + data.Be[2] * data.Be[2]);
mag_north->Be[0] = data.Be[0] / len;
mag_north->Be[1] = data.Be[1] / len;
mag_north->Be[2] = data.Be[2] / len;
}
}
static void load_baro_altitude(struct opahrs_msg_v1_req_update * update)
@ -389,7 +388,7 @@ static void load_baro_altitude(struct opahrs_msg_v1_req_update * update)
BaroAltitudeGet(&data);
update->barometer.altitude = data.Altitude;
update->barometer.updated = 1;
update->barometer.updated = TRUE;
}
static void load_gps_position(struct opahrs_msg_v1_req_update * update)
@ -399,18 +398,16 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
HomeLocationData home;
HomeLocationGet(&home);
update->gps.updated = 1;
update->gps.updated = TRUE;
if(home.RNE[0] == 0 && home.RNE[1] && home.RNE[2] == 0 && home.RNE[3] == 0)
{
if(home.RNE[0] == 0 && home.RNE[1] && home.RNE[2] == 0 && home.RNE[3] == 0) {
update->gps.NED[0] = 0;
update->gps.NED[1] = 0;
update->gps.NED[2] = 0;
update->gps.groundspeed = 0;
update->gps.heading = 0;
update->gps.quality = 0;
}
else {
} else {
update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading;
update->gps.quality = 0;

26
flight/PiOS/inc/pios_opahrs_proto.h
View File

@ -212,15 +212,15 @@ struct opahrs_msg_v1_req_attituderaw {
} __attribute__((__packed__));
struct opahrs_msg_v1_req_calibration {
uint8_t measure_var;
uint16_t accel_bias[3];
float accel_scale[3];
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
uint8_t measure_var;
uint16_t accel_bias[3];
float accel_scale[3];
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_scale[3];
float gyro_var[3];
uint16_t mag_bias[3];
float mag_var[3];
float mag_var[3];
} __attribute__((__packed__));
union opahrs_msg_v1_req {
@ -296,11 +296,11 @@ struct opahrs_msg_v1_rsp_update {
} __attribute__((__packed__));
struct opahrs_msg_v1_rsp_calibration {
uint8_t measure_var;
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_var[3];
float mag_var[3];
uint8_t measure_var;
float accel_var[3];
uint16_t gyro_bias[3];
float gyro_var[3];
float mag_var[3];
} __attribute__((__packed__));
union opahrs_msg_v1_rsp {