void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do {
SensorsRead(SENSOR_GYRO,1);
#ifndef OPTION_RC
DelayMsec(5);
#else
DelayMsec(16);
#endif
//printf("T:%d\n",getTickCount());
status=nvtGyroScaleCalibrate(axis);
led_arm_state(LED_STATE_TOGGLE);
UpdateLED();
} while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
void GyroCalibration()
{
const char axis_done = GetChar();
const char axis = axis_done - 0x78;
const char calibration_done = 'd';
signed char status;
nvtCalGyroInit(axis);
do
{
SensorsRead(SENSOR_GYRO,1);
DelayMsec(16);
status=nvtGyroScaleCalibrate(axis);
}
while(status==STATUS_GYRO_CAL_RUNNING);
if(status==STATUS_GYRO_AXIS_CAL_DONE)
Serial_write((char*)&axis_done, 1);
else
{
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_GYRO, false);
}
}
void AccCalibration()
{
const char direction = GetChar();
const char calibration_done = 'd';
char side = atoi(&direction);
signed char status;
if (direction == 'x')
side = 0;
else
side = atoi(&direction);
if ((direction == '0')||(direction == 'x'))
nvtCalACCInit();
do {
DelayMsec(1);
SensorsRead(SENSOR_ACC,1);
status = nvtCalACCBufferFill(side);
}while(status==STATUS_BUFFER_NOT_FILLED);
if(status==STATUS_BUFFER_FILLED) {
if (direction == 'x')
UpdateFlashCal(SENSOR_ACC, false);
Serial_write((char*)&direction, 1);
}
else {
Serial_write((char*)&calibration_done, 1);
UpdateFlashCal(SENSOR_ACC, false);
}
}
/*
* Serial_write_string
*
* Writes a string to the serial port one byte at a time.
*
* @param int port - the port ID
* @param char * data - the character array to be written
* @param int data_length - the length of the char array
* @return int - always returns 1.
*/
int Serial_write_string(int port, char * data, int data_length) {
int i = 0;
while( data[i] != 0x00) {
Serial_write(port, data[i]);
i++;
}
return 1;
}
void MagCalibration()
{
char calibration_done;
signed char status;
int16_t RawMAG[3];
nvtCalMAGInit();
do {
#ifndef OPTION_RC
DelayMsec(160);
#else
DelayMsec(320);
#endif
SensorsRead(SENSOR_MAG,1);
status = nvtCalMAGBufferFill();
nvtGetSensorRawMAG(RawMAG);
if (report_format == REPORT_FORMAT_BINARY) {
Serial_write((char*)RawMAG, 6);
}
else if (report_format == REPORT_FORMAT_TEXT) {
printf("@rM:%d,%d,%d\n",RawMAG[0],RawMAG[1],RawMAG[2]);
}
}while(status==STATUS_BUFFER_NOT_FILLED);
if(status==STATUS_CAL_DONE) {
CalQFactor = nvtGetMagCalQFactor();
if(CalQFactor<MAG_CAL_SUCESS_TH)
calibration_done = 'd';
else
calibration_done = 'f';
if (report_format == REPORT_FORMAT_BINARY) {
Serial_write((char*)&calibration_done, 1);
Serial_write((char*)&CalQFactor, 1);
}
else if (report_format == REPORT_FORMAT_TEXT) {
printf("%c,%d\n",calibration_done,CalQFactor);
}
}
}
int Serial_print(const char *format, ...)
{
int used_length;
char buf[SERIAL_PRINT_BUFSZ];
{
va_list vl;
va_start(vl, format);
used_length = vsnprintf(buf, SERIAL_PRINT_BUFSZ, format, vl);
va_end(vl);
}
Serial_write(buf);
return used_length;
}
void CommandProcess()
{
// Read incoming control messages
if (Serial_available() >= 2)
{
char start=Serial_read();
if (start == '@') {// Start of new control message
int command = Serial_read(); // Commands
if (command == 'h') {//Hook AHRS Stack Device
// Read ID
char id[2];
id[0] = GetChar();
id[1] = GetChar();
// Reply with synch message
printf("@HOOK");
Serial_write(id, 2);
}
else if (command == 'v') {//Check Version
CheckVersion();
}
else if (command == 'c') {// A 'c'calibration command
SensorCalibration();
}
else if (command == 'b') {// 'b'lock erase flash
FlashControl();
}
else if (command == 'p') {// Set 'p'id command
SetPID();
}
else if (command == 'm') {// Set report 'm'ode
char mode = GetChar();
if (mode == 'e') {// Report AHRS by 'e'uler angle
report_mode = REPORT_AHRS_EULER;
}
else if (mode == 'q') {// Report // Report AHRS by 'q'quaternion
report_mode = REPORT_AHRS_QUATERNION;
}
else if (mode == 'r') {// Report sensor 'r'aw data
report_mode = REPORT_SENSORS_RAW;
}
else if (mode == 'c') {// Report sensor 'c'alibrated data
report_mode = REPORT_SENSORS_CALIBRATED;
}
else if (mode == 'm') {// Report 'm'otor power distribution
report_mode = REPORT_MOTOR_POWER;
}
else if (mode == 'v') {// Report 'v'elocity
report_mode = REPORT_VELOCITY;
}
else if (mode == 's') {// Report 's'tatus
report_status();
}
else if (mode == 'p') {// Report controller 'p'id
char type = GetChar();
if (type == 'p') // 'p'id
report_mode = REPORT_PID;
else if (type == 'r') //'r'ate pid
report_mode = REPORT_RATE_PID;
else if (type == 'a') //'a'ltitude hold pid
report_mode = REPORT_ALTHOLD_PID;
}
}
else if (command == 'f') {// Set report 'f'ormat
char format = GetChar();
if (format == 'b') {// Report 'b'inary format
report_format = REPORT_FORMAT_BINARY;
}
else if (format == 't') {// Report 't'ext format
report_format = REPORT_FORMAT_TEXT;
}
}
else if (command == 's') {// 's'tream output control
char mode = GetChar();
if (mode == 's') {// 's'tart stream
stream_mode = STREAM_START;
}
else if (mode == 'p') {// 'p'ause stream
stream_mode = STREAM_PAUSE;
}
else if (mode == 't') {// 't'oggle stream
if(stream_mode==STREAM_START)
stream_mode = STREAM_PAUSE;
else
stream_mode = STREAM_START;
}
}
}
else {
if (report_format == REPORT_FORMAT_TEXT) {
printf("Unknown command.\n");
}
} // Skip character
}
}
void CalibrationFail()
{
const char fail = 'f';
Serial_write((char*)&fail, 1);
}
/* Model output function */
void testArduino_send_serial_output(void)
{
uint16_T rtb_AnalogInput_0;
uint16_T rtb_AnalogInput1_0;
uint16_T rtb_AnalogInput2_0;
uint8_T tmp[3];
real_T u;
real_T v;
if (testArduino_send_serial_M->Timing.TaskCounters.TID[1] == 0) {
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input' */
rtb_AnalogInput_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1' */
rtb_AnalogInput1_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput1_p1);
/* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2' */
rtb_AnalogInput2_0 = MW_analogRead(testArduino_send_serial_P.AnalogInput2_p1);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput_0 -
testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[0] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input1'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput1_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[1] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
/* DataTypeConversion: '<Root>/Data Type Conversion' incorporates:
* Constant: '<Root>/Constant'
* Constant: '<Root>/Constant1'
* Constant: '<Root>/Constant2'
* Product: '<Root>/Product'
* S-Function (arduinoanaloginput_sfcn): '<Root>/Analog Input2'
* Sum: '<Root>/Subtract'
* Sum: '<Root>/Subtract1'
*/
u = testArduino_send_serial_P.Constant2_Value - ((real_T)rtb_AnalogInput2_0
- testArduino_send_serial_P.Constant_Value) *
testArduino_send_serial_P.Constant1_Value;
v = fabs(u);
if (v < 4.503599627370496E+15) {
if (v >= 0.5) {
u = floor(u + 0.5);
} else {
u *= 0.0;
}
}
if (rtIsNaN(u) || rtIsInf(u)) {
u = 0.0;
} else {
u = fmod(u, 256.0);
}
/* S-Function (arduinoserialwrite_sfcn): '<Root>/Serial Transmit' incorporates:
* DataTypeConversion: '<Root>/Data Type Conversion'
* Sum: '<Root>/Subtract'
*/
tmp[2] = (uint8_T)(u < 0.0 ? (int16_T)(uint8_T)-(int8_T)(uint8_T)-u :
(int16_T)(uint8_T)u);
Serial_write(testArduino_send_serial_P.SerialTransmit_portNumber, tmp, 3UL);
}
}
/* Model output function */
void XbeeSerialRead_output(void)
{
uint8_T rtb_DataTypeConversion_j;
int16_T rtb_SerialReceive_o2;
uint8_T tmp[11];
/* S-Function (arduinoserialread_sfcn): '<Root>/Serial Receive' */
Serial_read(XbeeSerialRead_P.SerialReceive_p1,
XbeeSerialRead_P.SerialReceive_p2, &rtb_DataTypeConversion_j,
&rtb_SerialReceive_o2);
/* Switch: '<Root>/Switch' incorporates:
* UnitDelay: '<Root>/Unit Delay'
*/
if (rtb_SerialReceive_o2 > XbeeSerialRead_P.Switch_Threshold) {
XbeeSerialRead_B.Switch = rtb_DataTypeConversion_j;
} else {
XbeeSerialRead_B.Switch = XbeeSerialRead_DW.UnitDelay_DSTATE;
}
/* End of Switch: '<Root>/Switch' */
/* DataTypeConversion: '<S1>/Data Type Conversion' */
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
rtb_DataTypeConversion_j = (uint8_T)XbeeSerialRead_B.Switch;
} else {
rtb_DataTypeConversion_j = 0U;
}
} else {
rtb_DataTypeConversion_j = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S1>/Data Type Conversion' */
/* S-Function (arduinoanalogoutput_sfcn): '<S1>/PWM' */
MW_analogWrite(XbeeSerialRead_P.PWM_pinNumber, rtb_DataTypeConversion_j);
/* DataTypeConversion: '<S2>/Data Type Conversion' incorporates:
* Constant: '<S2>/Footer 1'
* Constant: '<S2>/Footer 2'
* Constant: '<S2>/Header'
*/
if (XbeeSerialRead_P.Header_Value < 256.0) {
if (XbeeSerialRead_P.Header_Value >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[0] = (uint8_T)XbeeSerialRead_P.Header_Value;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[0] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[0] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[1] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[1] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[1] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[2] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[2] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[2] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[3] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[3] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[3] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[4] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[4] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[4] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[5] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[5] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[5] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[6] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[6] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[6] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[7] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[7] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[7] = MAX_uint8_T;
}
if (XbeeSerialRead_B.Switch < 256.0) {
if (XbeeSerialRead_B.Switch >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[8] = (uint8_T)XbeeSerialRead_B.Switch;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[8] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[8] = MAX_uint8_T;
}
if (XbeeSerialRead_P.Footer1_Value < 256.0) {
if (XbeeSerialRead_P.Footer1_Value >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[9] = (uint8_T)XbeeSerialRead_P.Footer1_Value;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[9] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[9] = MAX_uint8_T;
}
if (XbeeSerialRead_P.Footer2_Value < 256.0) {
if (XbeeSerialRead_P.Footer2_Value >= 0.0) {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[10] = (uint8_T)XbeeSerialRead_P.Footer2_Value;
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[10] = 0U;
}
} else {
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
tmp[10] = MAX_uint8_T;
}
/* End of DataTypeConversion: '<S2>/Data Type Conversion' */
/* S-Function (arduinoserialwrite_sfcn): '<S2>/Serial Transmit' */
Serial_write(XbeeSerialRead_P.SerialTransmit_portNumber, tmp, 11UL);
}