From 47df1faa8d6300f45189e7c37bf54c123962f4e1 Mon Sep 17 00:00:00 2001 From: stac Date: Fri, 27 Aug 2010 02:14:58 +0000 Subject: [PATCH] ahrs: whitespace adjustments 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 --- flight/AHRS/ahrs.c | 60 +++++++------- .../OpenPilot/Modules/AHRSComms/ahrs_comms.c | 83 +++++++++---------- flight/PiOS/inc/pios_opahrs_proto.h | 26 +++--- 3 files changed, 83 insertions(+), 86 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 474bddcfa..58f2af80d 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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 diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index 04bf93dbd..b07e44351 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c @@ -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; diff --git a/flight/PiOS/inc/pios_opahrs_proto.h b/flight/PiOS/inc/pios_opahrs_proto.h index 3f57bad30..f1b9aeef0 100644 --- a/flight/PiOS/inc/pios_opahrs_proto.h +++ b/flight/PiOS/inc/pios_opahrs_proto.h @@ -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 {