/** 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);
}
