/** Sets the gyro bias * @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. * Length 3. * @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. */ void inv_set_gyro_bias(const long *bias, int accuracy) { if (bias != NULL) { if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); } } sensors.gyro.accuracy = accuracy; inv_data_builder.save.gyro_accuracy = accuracy; /* TODO: What should we do if there's no temperature data? */ if (sensors.temp.calibrated[0]) inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; else /* Set to 27 deg C for now until we've got a better solution. */ inv_data_builder.save.gyro_temp = 1769472L; inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); /* TODO: this flag works around the synchronization problem seen with using the user-exposed message layer to signal the temperature compensation module that gyro biases were set. A better, cleaner method is certainly needed. */ inv_data_builder.save.gyro_bias_tc_set = true; }
void inv_set_compass_bias(const long *bias, int accuracy) { if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); } sensors.compass.accuracy = accuracy; inv_data_builder.save.compass_accuracy = accuracy; inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); }
/** Sets the gyro bias * @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. * Length 3. * @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. */ void inv_set_gyro_bias(const long *bias, int accuracy) { if (bias != NULL) { if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); } } sensors.gyro.accuracy = accuracy; inv_data_builder.save.gyro_accuracy = accuracy; /* TODO: What should we do if there's no temperature data? */ if (sensors.temp.calibrated[0]) inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; else /* Set to 27 deg C for now until we've got a better solution. */ inv_data_builder.save.gyro_temp = 1769472L; inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); }
/** Sets the accel bias with control over which axis. * @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. * @param[in] mask Mask to select axis to apply bias set. */ void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) { if (bias) { if (mask & 1){ inv_data_builder.save.accel_bias[0] = bias[0]; } if (mask & 2){ inv_data_builder.save.accel_bias[1] = bias[1]; } if (mask & 4){ inv_data_builder.save.accel_bias[2] = bias[2]; } inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); } sensors.accel.accuracy = accuracy; inv_data_builder.save.accel_accuracy = accuracy; inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); }
/** Sets the motion state * @param[in] state motion state where INV_NO_MOTION is not moving * and INV_MOTION is moving. */ void inv_set_motion_state(unsigned char state) { long set; if (state == rh.motion_state) { if (state == INV_NO_MOTION) { rh.motion_state_counter++; } else { rh.motion_state_counter = 0; } return; } rh.motion_state_counter = 0; rh.motion_state = state; /* Equivalent to set = state, but #define's may change. */ if (state == INV_MOTION) set = INV_MSG_MOTION_EVENT; else set = INV_MSG_NO_MOTION_EVENT; inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); }
/** Sets the accel accuracy. * @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. */ void inv_set_accel_accuracy(int accuracy) { sensors.accel.accuracy = accuracy; inv_data_builder.save.accel_accuracy = accuracy; inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); }