| 250,7 → 250,7 |
|---|
| ROT_OFF; |
| |
| // get maximum and minimum reading of all axis |
| while (measurement && !MM3_Timeout) |
| while (measurement) |
| { |
| if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
| else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
| 276,26 → 276,24 |
|---|
| // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
| if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
| } |
| if(!MM3_Timeout) |
| { |
| // Rage of all axis |
| MM3_calib.X_range = (x_max - x_min); |
| MM3_calib.Y_range = (y_max - y_min); |
| MM3_calib.Z_range = (z_max - z_min); |
| // Rage of all axis |
| MM3_calib.X_range = (x_max - x_min); |
| MM3_calib.Y_range = (y_max - y_min); |
| MM3_calib.Z_range = (z_max - z_min); |
| |
| // Offset of all axis |
| MM3_calib.X_off = (x_max + x_min) / 2; |
| MM3_calib.Y_off = (y_max + y_min) / 2; |
| MM3_calib.Z_off = (z_max + z_min) / 2; |
| // Offset of all axis |
| MM3_calib.X_off = (x_max + x_min) / 2; |
| MM3_calib.Y_off = (y_max + y_min) / 2; |
| MM3_calib.Z_off = (z_max + z_min) / 2; |
| |
| // save to EEProm |
| SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
| SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
| SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
| SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
| SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
| SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
| } |
| // save to EEProm |
| SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
| SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
| SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
| SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
| SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
| SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
| |
| } |
| |
| |