├── serial ├── serial.layout ├── serial.cbp ├── main.c ├── rs232.h └── rs232.c ├── blurtoothConfig ├── serial.depend ├── serial.layout ├── serial.cbp ├── rs232.h ├── main.c └── rs232.c ├── newtonMethod ├── algorithm_newtonMethod.h ├── nm_prog.layout ├── nm_prog.cbp ├── nm_prog.depend ├── algorithm_matrix.h ├── main.c ├── algorithm_newtonMethod.c └── algorithm_matrix.c └── matrix ├── matrix_proj.layout ├── matrix_proj.cbp ├── main.c ├── matrix_proj.depend ├── matrix.h └── matrix.c /serial/serial.layout: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /blurtoothConfig/serial.depend: -------------------------------------------------------------------------------- 1 | # depslib dependency file v1.0 2 | 1420877081 source:c:\users\hom-msi\desktop\serial_20150902\rs232.c 3 | "rs232.h" 4 | 5 | 1464693585 c:\users\hom-msi\desktop\serial_20150902\rs232.h 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1464702899 source:c:\users\hom-msi\desktop\serial_20150902\main.c 18 | 19 | 20 | 21 | 22 | "rs232.h" 23 | 24 | -------------------------------------------------------------------------------- /newtonMethod/algorithm_newtonMethod.h: -------------------------------------------------------------------------------- 1 | /* #include "algorithm_newtonMethod.h" */ 2 | 3 | #ifndef __ALGORITHM_NEWTONMETHOD_H 4 | #define __ALGORITHM_NEWTONMETHOD_H 5 | 6 | #include 7 | /*====================================================================================================*/ 8 | /*====================================================================================================*/ 9 | void newtonMethod( const Matrix_TypeDef *matrixX, const Matrix_TypeDef *matrixA, const double alpha, const uint32_t n ); 10 | /*====================================================================================================*/ 11 | /*====================================================================================================*/ 12 | #endif 13 | -------------------------------------------------------------------------------- /matrix/matrix_proj.layout: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /blurtoothConfig/serial.layout: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /serial/serial.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 36 | 37 | -------------------------------------------------------------------------------- /blurtoothConfig/serial.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 36 | 37 | -------------------------------------------------------------------------------- /newtonMethod/nm_prog.layout: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /serial/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "rs232.h" 6 | 7 | #define RS232_ERROR 1 8 | #define RS232_SUCCESS 0 9 | 10 | #define RECV_BUF_SIZE 5 11 | 12 | int main() 13 | { 14 | int state = RS232_ERROR; 15 | 16 | int com_port = 6; /* COM7 */ 17 | int baudrate = 115200; /* 115200 */ 18 | char mode[] = {'8', 'N', '1', 0}; 19 | 20 | int i = 0, n = 0; 21 | unsigned long count = 2000000; 22 | 23 | unsigned char recv_buf[RECV_BUF_SIZE + 1] = {0}; 24 | 25 | /* Open COM Port */ 26 | state = RS232_OpenComport(com_port, baudrate, mode); 27 | if(!state) 28 | printf("Open COM Port Success\n\n"); 29 | 30 | do { 31 | n = RS232_PollComport(com_port, recv_buf, RECV_BUF_SIZE); 32 | // printf("count = %4i, n = %i, recv data = ", count, n); 33 | if(n == 5) { 34 | printf("count = %7i, n = %i, recv data = ", count, n); 35 | recv_buf[n] = '\0'; 36 | printf("%s", (char *)recv_buf); 37 | } 38 | else if(n > 0) { 39 | printf("count = %4i, n = %i, recv data = ", count, n); 40 | printf("*****\n"); 41 | } 42 | // Sleep(1); 43 | } while(--count); 44 | 45 | /* Close COM Port */ 46 | RS232_CloseComport(com_port); 47 | printf("\nClose COM Port\n"); 48 | 49 | return(0); 50 | } 51 | -------------------------------------------------------------------------------- /matrix/matrix_proj.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 49 | 50 | -------------------------------------------------------------------------------- /newtonMethod/nm_prog.cbp: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 53 | 54 | -------------------------------------------------------------------------------- /matrix/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "matrix.h" 5 | 6 | /* 7 | #define row_n 4 8 | #define col_n 8 9 | double array[row_n * col_n] = { 10 | 1.0, 3.0, -5.0, 2.0, 1.0, 0.0, 0.0, 0.0, 11 | 4.0, -3.0, 1.0, 5.0, 0.0, 1.0, 0.0, 0.0, 12 | 6.0, -2.0, 2.0, 4.0, 0.0, 0.0, 1.0, 0.0, 13 | 0.0, 2.0, 3.0, -7.0, 0.0, 0.0, 0.0, 1.0 14 | }; 15 | */ 16 | 17 | #define row_n 4 18 | #define col_n 4 19 | matrix_float_t arrayA[row_n * col_n] = { 20 | 1.0, 3.0, -5.0, 2.0, 21 | 4.0, -3.0, 1.0, 5.0, 22 | 6.0, -2.0, 2.0, 4.0, 23 | 0.0, 2.0, 3.0, -7.0, 24 | }; 25 | 26 | matrix_float_t arrayB[row_n * col_n] = { 27 | 0.000056, 3.0, -5.0, 72.0, 28 | 0.000115, -1.0, 1.0, 2120.0, 29 | 0.000841, -2.0, 8.0, 10.0, 30 | 0.000016, 2.0, 3.0, -86.0, 31 | }; 32 | 33 | int main() 34 | { 35 | matrix_t matrixA; 36 | Matrix_Init(&matrixA, arrayA, row_n, col_n); 37 | 38 | matrix_t *matrixB = Matrix_CreatePtr(arrayB, row_n, col_n); 39 | matrix_t *matrixC = Matrix_Create(row_n, col_n); 40 | Matrix_Clear(matrixC); 41 | 42 | printf("\n"); 43 | 44 | printf(" matrixA\n"); 45 | Matrix_Print(&matrixA); 46 | printf("\n"); 47 | 48 | printf(" matrixB\n"); 49 | Matrix_Print(matrixB); 50 | printf("\n"); 51 | 52 | printf(" matrixC\n"); 53 | Matrix_Print(matrixC); 54 | printf("\n"); 55 | 56 | // matrixC = matrixA * matrixB 57 | Matrix_Mul(matrixC, &matrixA, matrixB); 58 | printf(" matrixC = matrixA * matrixB\n"); 59 | Matrix_Print(matrixC); 60 | printf("\n"); 61 | 62 | // matrixC = inv(matrixC) 63 | Matrix_Inv(matrixC, matrixC); 64 | printf(" matrixC = inv(matrixC)\n"); 65 | Matrix_Print(matrixC); 66 | printf("\n"); 67 | 68 | Matrix_Delete(matrixB); 69 | Matrix_Delete(matrixC); 70 | } 71 | -------------------------------------------------------------------------------- /newtonMethod/nm_prog.depend: -------------------------------------------------------------------------------- 1 | # depslib dependency file v1.0 2 | 1448904798 source:c:\users\hom\desktop\matrix\algorithm_matrix.c 3 | 4 | 5 | "algorithm_matrix.h" 6 | 7 | 1448902458 c:\users\hom\desktop\matrix\algorithm_matrix.h 8 | 9 | 10 | 1448903181 source:c:\users\hom\desktop\matrix\main.c 11 | 12 | 13 | "algorithm_matrix.h" 14 | 15 | 1454335524 source:c:\users\hom\desktop\matrix_20151201\main.c 16 | 17 | 18 | "algorithm_matrix.h" 19 | 20 | 1448902458 c:\users\hom\desktop\matrix_20151201\algorithm_matrix.h 21 | 22 | 23 | 1454331343 source:c:\users\hom\desktop\matrix_20151201\algorithm_matrix.c 24 | 25 | 26 | 27 | "algorithm_matrix.h" 28 | 29 | 1454350882 source:c:\users\hom\desktop\newton_20151201\algorithm_matrix.c 30 | 31 | 32 | "algorithm_matrix.h" 33 | 34 | 1448902458 c:\users\hom\desktop\newton_20151201\algorithm_matrix.h 35 | 36 | 37 | 1454356920 source:c:\users\hom\desktop\newton_20151201\main.c 38 | 39 | 40 | "algorithm_matrix.h" 41 | 42 | 1454430582 source:c:\users\hom\desktop\newton_20160202\algorithm_matrix.c 43 | 44 | 45 | 46 | "algorithm_matrix.h" 47 | 48 | 1454428350 c:\users\hom\desktop\newton_20160202\algorithm_matrix.h 49 | 50 | 51 | 1454431535 source:c:\users\hom\desktop\newton_20160202\main.c 52 | 53 | 54 | "algorithm_matrix.h" 55 | 56 | 1454490281 source:c:\users\hom\desktop\newton_20160203\main.c 57 | 58 | 59 | "algorithm_matrix.h" 60 | "algorithm_newtonMethod.h" 61 | 62 | 1454439513 c:\users\hom\desktop\newton_20160203\algorithm_matrix.h 63 | 64 | 65 | 1454439824 source:c:\users\hom\desktop\newton_20160203\algorithm_matrix.c 66 | 67 | 68 | 69 | "algorithm_matrix.h" 70 | 71 | 1454490203 source:c:\users\hom\desktop\newton_20160203\algorithm_newtonmethod.c 72 | 73 | 74 | 75 | "algorithm_matrix.h" 76 | "algorithm_newtonMethod.h" 77 | 78 | 1454489975 c:\users\hom\desktop\newton_20160203\algorithm_newtonmethod.h 79 | 80 | 81 | -------------------------------------------------------------------------------- /serial/rs232.h: -------------------------------------------------------------------------------- 1 | /* 2 | *************************************************************************** 3 | * 4 | * Author: Teunis van Beelen 5 | * 6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen 7 | * 8 | * Email: teuniz@gmail.com 9 | * 10 | *************************************************************************** 11 | * 12 | * This program is free software; you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation version 2 of the License. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License along 22 | * with this program; if not, write to the Free Software Foundation, Inc., 23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 24 | * 25 | *************************************************************************** 26 | * 27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 28 | * 29 | *************************************************************************** 30 | */ 31 | 32 | /* Last revision: January 10, 2015 */ 33 | 34 | /* For more info and how to use this libray, visit: http://www.teuniz.net/RS-232/ */ 35 | 36 | 37 | #ifndef rs232_INCLUDED 38 | #define rs232_INCLUDED 39 | 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | 44 | #include 45 | #include 46 | 47 | 48 | 49 | #if defined(__linux__) || defined(__FreeBSD__) 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | #else 60 | 61 | #include 62 | 63 | #endif 64 | 65 | int RS232_OpenComport(int, int, const char *); 66 | int RS232_PollComport(int, unsigned char *, int); 67 | int RS232_SendByte(int, unsigned char); 68 | int RS232_SendBuf(int, unsigned char *, int); 69 | void RS232_CloseComport(int); 70 | void RS232_cputs(int, const char *); 71 | int RS232_IsDCDEnabled(int); 72 | int RS232_IsCTSEnabled(int); 73 | int RS232_IsDSREnabled(int); 74 | void RS232_enableDTR(int); 75 | void RS232_disableDTR(int); 76 | void RS232_enableRTS(int); 77 | void RS232_disableRTS(int); 78 | 79 | 80 | #ifdef __cplusplus 81 | } /* extern "C" */ 82 | #endif 83 | 84 | #endif 85 | 86 | 87 | -------------------------------------------------------------------------------- /blurtoothConfig/rs232.h: -------------------------------------------------------------------------------- 1 | /* 2 | *************************************************************************** 3 | * 4 | * Author: Teunis van Beelen 5 | * 6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen 7 | * 8 | * Email: teuniz@gmail.com 9 | * 10 | *************************************************************************** 11 | * 12 | * This program is free software; you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation version 2 of the License. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License along 22 | * with this program; if not, write to the Free Software Foundation, Inc., 23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 24 | * 25 | *************************************************************************** 26 | * 27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 28 | * 29 | *************************************************************************** 30 | */ 31 | 32 | /* Last revision: January 10, 2015 */ 33 | 34 | /* For more info and how to use this libray, visit: http://www.teuniz.net/RS-232/ */ 35 | 36 | 37 | #ifndef rs232_INCLUDED 38 | #define rs232_INCLUDED 39 | 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | 44 | #include 45 | #include 46 | 47 | 48 | 49 | #if defined(__linux__) || defined(__FreeBSD__) 50 | 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | 59 | #else 60 | 61 | #include 62 | 63 | #endif 64 | 65 | int RS232_OpenComport(int, int, const char *); 66 | int RS232_PollComport(int, unsigned char *, int); 67 | int RS232_SendByte(int, unsigned char); 68 | int RS232_SendBuf(int, unsigned char *, int); 69 | void RS232_CloseComport(int); 70 | void RS232_cputs(int, const char *); 71 | int RS232_IsDCDEnabled(int); 72 | int RS232_IsCTSEnabled(int); 73 | int RS232_IsDSREnabled(int); 74 | void RS232_enableDTR(int); 75 | void RS232_disableDTR(int); 76 | void RS232_enableRTS(int); 77 | void RS232_disableRTS(int); 78 | 79 | 80 | #ifdef __cplusplus 81 | } /* extern "C" */ 82 | #endif 83 | 84 | #endif 85 | 86 | 87 | -------------------------------------------------------------------------------- /matrix/matrix_proj.depend: -------------------------------------------------------------------------------- 1 | # depslib dependency file v1.0 2 | 1448904798 source:c:\users\hom\desktop\matrix\algorithm_matrix.c 3 | 4 | 5 | "algorithm_matrix.h" 6 | 7 | 1448902458 c:\users\hom\desktop\matrix\algorithm_matrix.h 8 | 9 | 10 | 1448903181 source:c:\users\hom\desktop\matrix\main.c 11 | 12 | 13 | "algorithm_matrix.h" 14 | 15 | 1454427028 source:c:\users\hom\desktop\matrix_20160202\main.c 16 | 17 | 18 | "algorithm_matrix.h" 19 | 20 | 1454426198 c:\users\hom\desktop\matrix_20160202\algorithm_matrix.h 21 | 22 | 23 | 1454427146 source:c:\users\hom\desktop\matrix_20160202\algorithm_matrix.c 24 | 25 | 26 | 27 | "algorithm_matrix.h" 28 | 29 | 1454439824 source:c:\users\hom\desktop\matrix_20160203\algorithm_matrix.c 30 | 31 | 32 | 33 | "algorithm_matrix.h" 34 | 35 | 1454439513 c:\users\hom\desktop\matrix_20160203\algorithm_matrix.h 36 | 37 | 38 | 1454439720 source:c:\users\hom\desktop\matrix_20160203\main.c 39 | 40 | 41 | "algorithm_matrix.h" 42 | 43 | 1466436870 source:d:\my_research\my_development\computer c\matrix_20160203\main.c 44 | 45 | 46 | "matrix.h" 47 | 48 | 1466436859 d:\my_research\my_development\computer c\matrix_20160203\matrix.h 49 | 50 | 51 | 52 | 1466436883 source:d:\my_research\my_development\computer c\matrix_20160203\matrix.c 53 | 54 | 55 | 56 | "matrix.h" 57 | 58 | 1466436870 source:d:\my_research\my_development\computer c\matrix_20160620\main.c 59 | 60 | 61 | "matrix.h" 62 | 63 | 1466436859 d:\my_research\my_development\computer c\matrix_20160620\matrix.h 64 | 65 | 66 | 67 | 1466436883 source:d:\my_research\my_development\computer c\matrix_20160620\matrix.c 68 | 69 | 70 | 71 | "matrix.h" 72 | 73 | 1466797863 source:d:\my_research\my_development\computer c\matrix_20160621\main.c 74 | 75 | 76 | "matrix.h" 77 | 78 | 1466796621 d:\my_research\my_development\computer c\matrix_20160621\matrix.h 79 | 80 | 81 | 82 | 1466797328 source:d:\my_research\my_development\computer c\matrix_20160621\matrix.c 83 | 84 | 85 | 86 | "matrix.h" 87 | 88 | 1466929345 source:d:\my_research\my_development\computer c\matrix_20160626\main.c 89 | 90 | 91 | "matrix.h" 92 | 93 | 1466929308 d:\my_research\my_development\computer c\matrix_20160626\matrix.h 94 | 95 | 96 | 97 | 1466929382 source:d:\my_research\my_development\computer c\matrix_20160626\matrix.c 98 | 99 | 100 | 101 | "matrix.h" 102 | 103 | -------------------------------------------------------------------------------- /matrix/matrix.h: -------------------------------------------------------------------------------- 1 | /* #include "matrix.h" */ 2 | 3 | #ifndef __MATRIX_H 4 | #define __MATRIX_H 5 | 6 | #include 7 | #include 8 | /*====================================================================================================*/ 9 | /*====================================================================================================*/ 10 | typedef double matrix_float_t; 11 | 12 | typedef enum { 13 | MTYPE_NORMAL = 0, 14 | MTYPE_MALLOC, 15 | MTYPE_POINTER 16 | } matrix_mType_t; 17 | 18 | typedef struct { 19 | uint16_t rows; 20 | uint16_t cols; 21 | matrix_float_t *arr; 22 | matrix_mType_t mType; 23 | } matrix_t; 24 | /*====================================================================================================*/ 25 | /*====================================================================================================*/ 26 | void Matrix_Init( matrix_t *pMatrix, matrix_float_t *pArray, uint16_t rows, uint16_t cols ); 27 | void Matrix_Clear( matrix_t *pMatrix ); 28 | matrix_t *Matrix_Create( uint16_t rows, uint16_t cols ); 29 | matrix_t *Matrix_CreatePtr( matrix_float_t *pArray, uint16_t rows, uint16_t cols ); 30 | matrix_t *Matrix_CreateDiag( uint16_t cols, matrix_float_t data ); 31 | void Matrix_Delete( matrix_t *matrix ); 32 | void Matrix_Copy( matrix_t *pMatrixC1, matrix_t *pMatrixC2 ); 33 | matrix_t *Matrix_CopyMatrix( matrix_t *pMatrix ); 34 | matrix_t *Matrix_CopyArray( matrix_float_t *pArray, uint16_t rows, uint16_t cols ); 35 | void Matrix_Resize( matrix_t *pMatrix, uint16_t rows, uint16_t cols ); 36 | void Matrix_SetData( matrix_t *pMatrix, uint16_t rows, uint16_t cols, matrix_float_t data ); 37 | matrix_float_t Matrix_GetData( matrix_t *pMatrix, uint16_t rows, uint16_t cols ); 38 | void Matrix_SetDiag( matrix_t *pMatrix, matrix_float_t data ); 39 | void Matrix_GetDiag( matrix_t *pMatrix, matrix_t *pMatrixD ); 40 | void Matrix_SetMatrix( matrix_t *pMatrix, matrix_t *pMatrixS, uint16_t rows_pos, uint16_t cols_pos ); 41 | void Matrix_GetMatrix( matrix_t *pMatrixG, matrix_t *pMatrix, uint16_t rows_pos, uint16_t cols_pos ); 42 | void Matrix_Add( matrix_t *pMatrix, matrix_t *pMatrixA1, matrix_t *pMatrixA2 ); 43 | void Matrix_Sub( matrix_t *pMatrix, matrix_t *pMatrixS1, matrix_t *pMatrixS2 ); 44 | void Matrix_Mul( matrix_t *pMatrix, matrix_t *pMatrixM1, matrix_t *pMatrixM2 ); 45 | void Matrix_MulNumb( matrix_t *pMatrix, matrix_t *pMatrixM1, matrix_float_t number ); 46 | void Matrix_Transpose( matrix_t *pMatrix, matrix_t *pMatrixT ); 47 | void Matrix_Inv( matrix_t *pMatrix, matrix_t *pMatrixInv ); 48 | void Matrix_GaussianElimination( matrix_t *pMatrix, matrix_t *pMatrixGE ); 49 | matrix_float_t Matrix_Det( matrix_t *pMatrix ); 50 | void Matrix_Cholesky( matrix_t *pMatrix, matrix_t *pMatrixC ); 51 | void Matrix_Print( matrix_t *pMatrix ); 52 | void Matrix_PrintInfo( matrix_t *pMatrix ); 53 | /*====================================================================================================*/ 54 | /*====================================================================================================*/ 55 | #endif 56 | -------------------------------------------------------------------------------- /newtonMethod/algorithm_matrix.h: -------------------------------------------------------------------------------- 1 | /* #include "algorithm_matrix.h" */ 2 | 3 | #ifndef __ALGORITHM_MATRIX_H 4 | #define __ALGORITHM_MATRIX_H 5 | 6 | #include 7 | /*====================================================================================================*/ 8 | /*====================================================================================================*/ 9 | typedef enum { 10 | MSTATE_MALLOC = 0, 11 | MSTATE_POINTER, 12 | } Matrix_mState_TypeDef; 13 | 14 | typedef struct { 15 | uint16_t rows; 16 | uint16_t cols; 17 | uint32_t total; 18 | double *arr; 19 | Matrix_mState_TypeDef mState; 20 | } Matrix_TypeDef; 21 | /*====================================================================================================*/ 22 | /*====================================================================================================*/ 23 | void Matrix_init( Matrix_TypeDef *pMatrix, double *pArray, uint16_t rows, uint16_t cols ); 24 | void Matrix_clear( Matrix_TypeDef *pMatrix ); 25 | Matrix_TypeDef *Matrix_create( uint16_t rows, uint16_t cols ); 26 | Matrix_TypeDef *Matrix_createPtr( double *pArray, uint16_t rows, uint16_t cols ); 27 | void Matrix_delete( Matrix_TypeDef *matrix ); 28 | Matrix_TypeDef *Matrix_createDiag( uint16_t cols, double data ); 29 | void Matrix_copy( Matrix_TypeDef *pMatrixC1, Matrix_TypeDef *pMatrixC2 ); 30 | Matrix_TypeDef *Matrix_copyMatrix( Matrix_TypeDef *pMatrix ); 31 | Matrix_TypeDef *Matrix_copyArray( double *pArray, uint16_t rows, uint16_t cols ); 32 | void Matrix_resize( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols ); 33 | void Matrix_setData( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols, double data ); 34 | double Matrix_getData( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols ); 35 | void Matrix_setDiag( Matrix_TypeDef *pMatrix, double data ); 36 | void Matrix_getDiag( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixD ); 37 | void Matrix_setMatrix( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixS, uint16_t rows_pos, uint16_t cols_pos ); 38 | void Matrix_getMatrix( Matrix_TypeDef *pMatrixG, Matrix_TypeDef *pMatrix, uint16_t rows_pos, uint16_t cols_pos ); 39 | void Matrix_add( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixA1, Matrix_TypeDef *pMatrixA2 ); 40 | void Matrix_sub( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixS1, Matrix_TypeDef *pMatrixS2 ); 41 | void Matrix_mul( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixM1, Matrix_TypeDef *pMatrixM2 ); 42 | void Matrix_mulNumb( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixM1, double number ); 43 | void Matrix_transpose( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixT ); 44 | void Matrix_inv( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixInv ); 45 | void Matrix_gaussianElimination( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixGE ); 46 | double Matrix_det( Matrix_TypeDef *pMatrix ); 47 | void Matrix_cholesky( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixC ); 48 | void Matrix_print( Matrix_TypeDef *pMatrix ); 49 | /*====================================================================================================*/ 50 | /*====================================================================================================*/ 51 | #endif 52 | -------------------------------------------------------------------------------- /blurtoothConfig/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "rs232.h" 7 | 8 | #define BUF_SIZE 100 9 | 10 | int com_port = 6; /* COM7 */ 11 | int baudrate = 38400; /* 115200 */ 12 | char mode[] = {'8', 'N', '1', 0}; 13 | 14 | int i = 0, n = 0; 15 | unsigned char sendBuf[BUF_SIZE] = {0}; 16 | unsigned char recvBuf[BUF_SIZE] = {0}; 17 | 18 | #define HC05 5 19 | #define HC06 6 20 | void bt_sendCommand( char type, char* pString ) 21 | { 22 | // printf(pString); 23 | // printf(" ...\t"); 24 | // printf("\n"); 25 | RS232_SendBuf(com_port, pString, strlen(pString)); 26 | if(type == HC05) 27 | RS232_SendBuf(com_port, "\r\n", 2); 28 | Sleep(100); 29 | } 30 | 31 | void bt_sendBIND( char type, char* pString ) 32 | { 33 | RS232_SendBuf(com_port, "AT+BIND=", strlen("AT+BIND=")); 34 | RS232_SendBuf(com_port, pString, strlen(pString)); 35 | RS232_SendBuf(com_port, "\r\n", 2); 36 | Sleep(100); 37 | } 38 | 39 | void bt_getReturn( void ) 40 | { 41 | int lens = 0; 42 | unsigned char pString[128] = {0}; 43 | 44 | lens = RS232_PollComport(com_port, pString, 100); 45 | // printf("[%i]\n", lens); 46 | printf(pString); 47 | printf("\n"); 48 | Sleep(100); 49 | } 50 | 51 | void bt_getReturnINQ( void ) 52 | { 53 | int lens = 0; 54 | unsigned char pString[128] = {0}; 55 | 56 | lens = RS232_PollComport(com_port, pString, 100); 57 | printf("[%i]\n", lens); 58 | printf(pString); 59 | printf("\n"); 60 | } 61 | 62 | void bt_sendATCommand( char type, char* pString ) 63 | { 64 | if(strcmp("AT+INQ", pString) != 0) { 65 | bt_sendCommand(type, pString); 66 | bt_getReturn(); 67 | } 68 | else { 69 | bt_sendCommand(type, pString); 70 | Sleep(500); 71 | bt_getReturnINQ(); 72 | } 73 | } 74 | int main() 75 | { 76 | char HC0x = HC05; 77 | char pString[128] = {0}; 78 | 79 | printf("DeviceType : "); 80 | scanf ("%d", &HC0x); 81 | if(HC0x == HC05) { 82 | baudrate = 38400; 83 | } 84 | else { 85 | printf("Baudrate : "); 86 | scanf ("%d", &baudrate); 87 | } 88 | 89 | /* Open COM Port */ 90 | if(!RS232_OpenComport(com_port, baudrate, mode)) { 91 | printf("Open COM Port Success\n\n"); 92 | 93 | do { 94 | printf("AT Command : "); 95 | scanf ("%s",pString); 96 | bt_sendATCommand(HC0x, pString); 97 | } while(strcmp("EXTI", pString) != 0); 98 | 99 | bt_sendATCommand(HC0x, "AT"); 100 | bt_sendATCommand(HC0x, "AT+ORGL"); // 101 | bt_sendATCommand(HC0x, "AT+ROLE=1"); // 102 | bt_sendATCommand(HC0x, "AT+ROLE?"); // 103 | bt_sendATCommand(HC0x, "AT+PSWD=0000"); // 104 | bt_sendATCommand(HC0x, "AT+PSWD?"); 105 | bt_sendATCommand(HC0x, "AT+UART=115200,0,0"); 106 | bt_sendATCommand(HC0x, "AT+UART?"); 107 | bt_sendATCommand(HC0x, "AT+CMODE=0"); 108 | bt_sendATCommand(HC0x, "AT+CMODE?"); 109 | printf("AT+BIND="); 110 | scanf ("%s",pString); 111 | bt_sendBIND(HC0x, pString); 112 | bt_sendATCommand(HC0x, "AT+BIND?"); 113 | } 114 | else { 115 | printf("Open COM Port Error\n"); 116 | } 117 | 118 | /* Close COM Port */ 119 | RS232_CloseComport(com_port); 120 | printf("\n\nClose COM Port\n"); 121 | 122 | return(0); 123 | } 124 | -------------------------------------------------------------------------------- /newtonMethod/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "algorithm_matrix.h" 5 | #include "algorithm_newtonMethod.h" 6 | 7 | #define row_A 3 8 | #define col_A 18 9 | double arrayA[row_A * col_A] = { 10 | 1.0000, -1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.7070, 0.7070, -0.7070, -0.7070, 0.7070, 0.7070, -0.7070, -0.7070, 0.0000, 0.0000, 0.0000, 0.0000, 11 | 0.0000, 0.0000, 1.0000, -1.0000, 0.0000, 0.0000, 0.7070, -0.7070, -0.7070, 0.7070, 0.0000, 0.0000, 0.0000, 0.0000, 0.7070, 0.7070, -0.7070, -0.7070, 12 | 0.0000, 0.0000, 0.0000, 0.0000, 1.0000, -1.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.7070, -0.7070, -0.7070, 0.7070, 0.7070, -0.7070, -0.7070, 0.7070, 13 | }; 14 | 15 | #define row_K 3 16 | #define col_K 3 17 | double arrayK[row_K * col_K] = { 18 | 0.9980, -0.0109, 0.0293, 19 | -0.0109, 1.0419, 0.0509, 20 | 0.0293, 0.0509, 0.9552, 21 | }; 22 | 23 | #define row_B 3 24 | #define col_B 1 25 | double arrayB[row_B * col_B] = { 26 | 35.9405, 27 | 31.0196, 28 | -67.4777, 29 | }; 30 | 31 | #define row_X 9 32 | #define col_X 1 33 | double arrayX[row_X * col_X] = { 34 | 1.0, 35 | 0.0, 36 | 0.0, 37 | 1.0, 38 | 0.0, 39 | 1.0, 40 | 0.0, 41 | 0.0, 42 | 0.0, 43 | }; 44 | 45 | int main() 46 | { 47 | Matrix_TypeDef *matrixA = Matrix_createPtr(arrayA, row_A, col_A); 48 | Matrix_TypeDef *matrixK = Matrix_createPtr(arrayK, row_K, col_K); 49 | Matrix_TypeDef *matrixS = Matrix_create(row_K, col_K); 50 | Matrix_TypeDef *matrixB = Matrix_createPtr(arrayB, row_B, col_B); 51 | Matrix_TypeDef *matrixX = Matrix_createPtr(arrayX, row_X, col_X); 52 | 53 | // cal noiseA 54 | Matrix_inv(matrixS, matrixK); // K = inv(K) 55 | for(uint8_t i = 0; i < col_A; i++) { 56 | Matrix_TypeDef *tmpMatrixA = Matrix_create(row_A, 1); 57 | 58 | Matrix_getMatrix(tmpMatrixA, matrixA, 0, i); // tmpA = A(:, i) 59 | Matrix_mulNumb(tmpMatrixA, tmpMatrixA, 1000); // tmpA = tmpA * 1000 60 | Matrix_mul(tmpMatrixA, matrixS, tmpMatrixA); // tmpA = K * tmpA 61 | Matrix_add(tmpMatrixA, tmpMatrixA, matrixB); // tmpA = tmpA + b 62 | Matrix_setMatrix(matrixA, tmpMatrixA, 0, i); // A(:, i) = tmpA 63 | 64 | Matrix_delete(tmpMatrixA); 65 | } 66 | 67 | // gauss-newton method 68 | newtonMethod(matrixX, matrixA, 0.32, 100); 69 | 70 | // print result 71 | printf("\n\t origin\t\t newton\t\t(newton - origin) / origin * 100\n"); 72 | printf("k_11\t%.6f\t%.6f\terr = %.24f\n", arrayK[0], arrayX[0], (arrayX[0] - arrayK[0]) / arrayK[0] * 100); 73 | printf("k_12\t%.6f\t%.6f\terr = %.24f\n", arrayK[1], arrayX[1], (arrayX[1] - arrayK[1]) / arrayK[1] * 100); 74 | printf("k_13\t%.6f\t%.6f\terr = %.24f\n", arrayK[2], arrayX[2], (arrayX[2] - arrayK[2]) / arrayK[2] * 100); 75 | printf("k_22\t%.6f\t%.6f\terr = %.24f\n", arrayK[4], arrayX[3], (arrayX[3] - arrayK[4]) / arrayK[4] * 100); 76 | printf("k_23\t%.6f\t%.6f\terr = %.24f\n", arrayK[5], arrayX[4], (arrayX[4] - arrayK[5]) / arrayK[5] * 100); 77 | printf("k_33\t%.6f\t%.6f\terr = %.24f\n", arrayK[8], arrayX[5], (arrayX[5] - arrayK[8]) / arrayK[8] * 100); 78 | printf("b_x \t%.6f\t%.6f\terr = %.24f\n", arrayB[0], arrayX[6], (arrayX[6] - arrayB[0]) / arrayB[0] * 100); 79 | printf("b_y \t%.6f\t%.6f\terr = %.24f\n", arrayB[1], arrayX[7], (arrayX[7] - arrayB[1]) / arrayB[1] * 100); 80 | printf("b_z \t%.6f\t%.6f\terr = %.24f\n", arrayB[2], arrayX[8], (arrayX[8] - arrayB[2]) / arrayB[2] * 100); 81 | 82 | Matrix_delete(matrixA); 83 | Matrix_delete(matrixK); 84 | Matrix_delete(matrixS); 85 | Matrix_delete(matrixB); 86 | Matrix_delete(matrixX); 87 | } 88 | /*====================================================================================================*/ 89 | /*====================================================================================================*/ 90 | -------------------------------------------------------------------------------- /newtonMethod/algorithm_newtonMethod.c: -------------------------------------------------------------------------------- 1 | /*====================================================================================================*/ 2 | /*====================================================================================================*/ 3 | #include 4 | #include 5 | #include 6 | 7 | #include "algorithm_matrix.h" 8 | #include "algorithm_newtonMethod.h" 9 | 10 | #define row_X 9 11 | #define col_X 1 12 | /*====================================================================================================*/ 13 | /*====================================================================================================*/ 14 | void newtonMethod_G( const Matrix_TypeDef *matrixX, const double *pAccel, const uint16_t n, Matrix_TypeDef *matrixG ); 15 | /*====================================================================================================*/ 16 | /*====================================================================================================* 17 | **函數 : newtonMethod 18 | **功能 : 19 | **輸入 : *matrixX, *pAccel, alpha, n 20 | **輸出 : none 21 | **使用 : newtonMethod(matrixX, matrixA, alpha, n); 22 | **====================================================================================================*/ 23 | /*====================================================================================================*/ 24 | void newtonMethod( const Matrix_TypeDef *matrixX, const Matrix_TypeDef *matrixA, const double alpha, const uint32_t n ) 25 | { 26 | Matrix_TypeDef *matrixG = Matrix_create(matrixX->rows, matrixX->cols); 27 | 28 | // gauss-newton method 29 | for(uint32_t i = 0; i < n; i++) { 30 | newtonMethod_G(matrixX, matrixA->arr, matrixA->cols, matrixG); 31 | Matrix_mulNumb(matrixG, matrixG, alpha); 32 | Matrix_sub(matrixX, matrixX, matrixG); 33 | } 34 | 35 | Matrix_delete(matrixG); 36 | } 37 | /*====================================================================================================*/ 38 | /*====================================================================================================*/ 39 | void newtonMethod_e( const double *arrayX, const double *pAccel, double *param_e ) 40 | { 41 | double accel[3] = {0}; 42 | double param[3] = {0}; 43 | 44 | for(uint8_t i = 0; i < 3; i++) 45 | accel[i] = pAccel[i] - arrayX[6 + i]; 46 | 47 | param[0] = arrayX[0] * accel[0] + arrayX[1] * accel[1] + arrayX[2] * accel[2]; 48 | param[1] = arrayX[1] * accel[0] + arrayX[3] * accel[1] + arrayX[4] * accel[2]; 49 | param[2] = arrayX[2] * accel[0] + arrayX[4] * accel[1] + arrayX[5] * accel[2]; 50 | 51 | *param_e = param[0] * param[0] + param[1] * param[1] + param[2] * param[2] - 1000000.0; 52 | } 53 | /*====================================================================================================*/ 54 | /*====================================================================================================*/ 55 | void newtonMethod_Ge( const double *arrayX, const double *pAccel, double *param_Ge ) 56 | { 57 | double param_A = arrayX[0] * arrayX[0] + arrayX[1] * arrayX[1] + arrayX[2] * arrayX[2]; 58 | double param_B = arrayX[1] * arrayX[1] + arrayX[3] * arrayX[3] + arrayX[4] * arrayX[4]; 59 | double param_C = arrayX[2] * arrayX[2] + arrayX[4] * arrayX[4] + arrayX[5] * arrayX[5]; 60 | double param_D = arrayX[0] * arrayX[1] + arrayX[1] * arrayX[3] + arrayX[2] * arrayX[4]; 61 | double param_E = arrayX[1] * arrayX[2] + arrayX[3] * arrayX[4] + arrayX[4] * arrayX[5]; 62 | double param_F = arrayX[0] * arrayX[2] + arrayX[1] * arrayX[4] + arrayX[2] * arrayX[5]; 63 | 64 | double accel[3] = {0}; 65 | for(uint8_t i = 0; i < 3; i++) 66 | accel[i] = pAccel[i] - arrayX[6 + i]; 67 | 68 | double param_a1a1 = accel[0] * accel[0]; 69 | double param_a1a2 = accel[0] * accel[1]; 70 | double param_a1a3 = accel[0] * accel[2]; 71 | double param_a2a2 = accel[1] * accel[1]; 72 | double param_a2a3 = accel[1] * accel[2]; 73 | double param_a3a3 = accel[2] * accel[2]; 74 | 75 | param_Ge[0] = arrayX[0] * param_a1a1 + arrayX[1] * param_a1a2 + arrayX[2] * param_a1a3; 76 | param_Ge[1] = arrayX[1] * (param_a1a1 + param_a2a2) + (arrayX[0] + arrayX[3]) * param_a1a2 + arrayX[2] * param_a2a3 + arrayX[4] * param_a1a3; 77 | param_Ge[2] = arrayX[2] * (param_a1a1 + param_a3a3) + arrayX[4] * param_a1a2 + arrayX[1] * param_a2a3 + (arrayX[0] + arrayX[5]) * param_a1a3; 78 | param_Ge[3] = arrayX[1] * param_a1a2 + arrayX[3] * param_a2a2 + arrayX[4] * param_a2a3; 79 | param_Ge[4] = arrayX[4] * (param_a2a2 + param_a3a3) + arrayX[2] * param_a1a2 + (arrayX[3] + arrayX[5]) * param_a2a3 + arrayX[1] * param_a1a3; 80 | param_Ge[5] = arrayX[2] * param_a1a3 + arrayX[4] * param_a2a3 + arrayX[5] * param_a3a3; 81 | param_Ge[6] = - param_A * accel[0] - param_D * accel[1] - param_F * accel[2]; 82 | param_Ge[7] = - param_D * accel[0] - param_B * accel[1] - param_E * accel[2]; 83 | param_Ge[8] = - param_F * accel[0] - param_E * accel[1] - param_C * accel[2]; 84 | } 85 | /*====================================================================================================*/ 86 | /*====================================================================================================*/ 87 | void newtonMethod_G( const Matrix_TypeDef *matrixX, const double *pAccel, const uint16_t n, Matrix_TypeDef *matrixG ) 88 | { 89 | double accel[3] = {0}; 90 | double param_e = 0; 91 | double param_Ge[row_X * col_X] = {0}; 92 | 93 | Matrix_TypeDef *matrixGE = Matrix_create(row_X, col_X); 94 | Matrix_TypeDef *matrixG2E = Matrix_create(row_X, row_X); 95 | 96 | for(uint8_t i = 0; i < n; i++) { 97 | accel[0] = pAccel[0 * n + i]; 98 | accel[1] = pAccel[1 * n + i]; 99 | accel[2] = pAccel[2 * n + i]; 100 | newtonMethod_e(matrixX->arr, accel, ¶m_e); 101 | newtonMethod_Ge(matrixX->arr, accel, param_Ge); 102 | 103 | // G2E = sum(Ge^T * Ge) 104 | for(uint8_t j = 0; j < row_X; j++) { 105 | matrixGE->arr[j] += param_e * param_Ge[j]; 106 | for(uint8_t k = 0; k < row_X; k++) { 107 | matrixG2E->arr[k * row_X + j] += param_Ge[j] * param_Ge[k]; 108 | } 109 | } 110 | } 111 | 112 | Matrix_inv(matrixG2E, matrixG2E); 113 | Matrix_mul(matrixG, matrixG2E, matrixGE); 114 | 115 | Matrix_delete(matrixGE); 116 | Matrix_delete(matrixG2E); 117 | } 118 | /*====================================================================================================*/ 119 | /*====================================================================================================*/ 120 | -------------------------------------------------------------------------------- /serial/rs232.c: -------------------------------------------------------------------------------- 1 | /* 2 | *************************************************************************** 3 | * 4 | * Author: Teunis van Beelen 5 | * 6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen 7 | * 8 | * Email: teuniz@gmail.com 9 | * 10 | *************************************************************************** 11 | * 12 | * This program is free software; you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation version 2 of the License. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License along 22 | * with this program; if not, write to the Free Software Foundation, Inc., 23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 24 | * 25 | *************************************************************************** 26 | * 27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 28 | * 29 | *************************************************************************** 30 | */ 31 | 32 | /* Last revision: January 10, 2015 */ 33 | 34 | /* For more info and how to use this library, visit: http://www.teuniz.net/RS-232/ */ 35 | 36 | 37 | #include "rs232.h" 38 | 39 | 40 | 41 | #if defined(__linux__) || defined(__FreeBSD__) /* Linux & FreeBSD */ 42 | 43 | 44 | int Cport[38], 45 | error; 46 | 47 | struct termios new_port_settings, 48 | old_port_settings[38]; 49 | 50 | char comports[38][16]={"/dev/ttyS0","/dev/ttyS1","/dev/ttyS2","/dev/ttyS3","/dev/ttyS4","/dev/ttyS5", 51 | "/dev/ttyS6","/dev/ttyS7","/dev/ttyS8","/dev/ttyS9","/dev/ttyS10","/dev/ttyS11", 52 | "/dev/ttyS12","/dev/ttyS13","/dev/ttyS14","/dev/ttyS15","/dev/ttyUSB0", 53 | "/dev/ttyUSB1","/dev/ttyUSB2","/dev/ttyUSB3","/dev/ttyUSB4","/dev/ttyUSB5", 54 | "/dev/ttyAMA0","/dev/ttyAMA1","/dev/ttyACM0","/dev/ttyACM1", 55 | "/dev/rfcomm0","/dev/rfcomm1","/dev/ircomm0","/dev/ircomm1", 56 | "/dev/cuau0","/dev/cuau1","/dev/cuau2","/dev/cuau3", 57 | "/dev/cuaU0","/dev/cuaU1","/dev/cuaU2","/dev/cuaU3"}; 58 | 59 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode) 60 | { 61 | int baudr, 62 | status; 63 | 64 | if((comport_number>37)||(comport_number<0)) 65 | { 66 | printf("illegal comport number\n"); 67 | return(1); 68 | } 69 | 70 | switch(baudrate) 71 | { 72 | case 50 : baudr = B50; 73 | break; 74 | case 75 : baudr = B75; 75 | break; 76 | case 110 : baudr = B110; 77 | break; 78 | case 134 : baudr = B134; 79 | break; 80 | case 150 : baudr = B150; 81 | break; 82 | case 200 : baudr = B200; 83 | break; 84 | case 300 : baudr = B300; 85 | break; 86 | case 600 : baudr = B600; 87 | break; 88 | case 1200 : baudr = B1200; 89 | break; 90 | case 1800 : baudr = B1800; 91 | break; 92 | case 2400 : baudr = B2400; 93 | break; 94 | case 4800 : baudr = B4800; 95 | break; 96 | case 9600 : baudr = B9600; 97 | break; 98 | case 19200 : baudr = B19200; 99 | break; 100 | case 38400 : baudr = B38400; 101 | break; 102 | case 57600 : baudr = B57600; 103 | break; 104 | case 115200 : baudr = B115200; 105 | break; 106 | case 230400 : baudr = B230400; 107 | break; 108 | case 460800 : baudr = B460800; 109 | break; 110 | case 500000 : baudr = B500000; 111 | break; 112 | case 576000 : baudr = B576000; 113 | break; 114 | case 921600 : baudr = B921600; 115 | break; 116 | case 1000000 : baudr = B1000000; 117 | break; 118 | case 1152000 : baudr = B1152000; 119 | break; 120 | case 1500000 : baudr = B1500000; 121 | break; 122 | case 2000000 : baudr = B2000000; 123 | break; 124 | case 2500000 : baudr = B2500000; 125 | break; 126 | case 3000000 : baudr = B3000000; 127 | break; 128 | case 3500000 : baudr = B3500000; 129 | break; 130 | case 4000000 : baudr = B4000000; 131 | break; 132 | default : printf("invalid baudrate\n"); 133 | return(1); 134 | break; 135 | } 136 | 137 | int cbits=CS8, 138 | cpar=0, 139 | ipar=IGNPAR, 140 | bstop=0; 141 | 142 | if(strlen(mode) != 3) 143 | { 144 | printf("invalid mode \"%s\"\n", mode); 145 | return(1); 146 | } 147 | 148 | switch(mode[0]) 149 | { 150 | case '8': cbits = CS8; 151 | break; 152 | case '7': cbits = CS7; 153 | break; 154 | case '6': cbits = CS6; 155 | break; 156 | case '5': cbits = CS5; 157 | break; 158 | default : printf("invalid number of data-bits '%c'\n", mode[0]); 159 | return(1); 160 | break; 161 | } 162 | 163 | switch(mode[1]) 164 | { 165 | case 'N': 166 | case 'n': cpar = 0; 167 | ipar = IGNPAR; 168 | break; 169 | case 'E': 170 | case 'e': cpar = PARENB; 171 | ipar = INPCK; 172 | break; 173 | case 'O': 174 | case 'o': cpar = (PARENB | PARODD); 175 | ipar = INPCK; 176 | break; 177 | default : printf("invalid parity '%c'\n", mode[1]); 178 | return(1); 179 | break; 180 | } 181 | 182 | switch(mode[2]) 183 | { 184 | case '1': bstop = 0; 185 | break; 186 | case '2': bstop = CSTOPB; 187 | break; 188 | default : printf("invalid number of stop bits '%c'\n", mode[2]); 189 | return(1); 190 | break; 191 | } 192 | 193 | /* 194 | http://pubs.opengroup.org/onlinepubs/7908799/xsh/termios.h.html 195 | 196 | http://man7.org/linux/man-pages/man3/termios.3.html 197 | */ 198 | 199 | Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY); 200 | if(Cport[comport_number]==-1) 201 | { 202 | perror("unable to open comport "); 203 | return(1); 204 | } 205 | 206 | error = tcgetattr(Cport[comport_number], old_port_settings + comport_number); 207 | if(error==-1) 208 | { 209 | close(Cport[comport_number]); 210 | perror("unable to read portsettings "); 211 | return(1); 212 | } 213 | memset(&new_port_settings, 0, sizeof(new_port_settings)); /* clear the new struct */ 214 | 215 | new_port_settings.c_cflag = cbits | cpar | bstop | CLOCAL | CREAD; 216 | new_port_settings.c_iflag = ipar; 217 | new_port_settings.c_oflag = 0; 218 | new_port_settings.c_lflag = 0; 219 | new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ 220 | new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ 221 | 222 | cfsetispeed(&new_port_settings, baudr); 223 | cfsetospeed(&new_port_settings, baudr); 224 | 225 | error = tcsetattr(Cport[comport_number], TCSANOW, &new_port_settings); 226 | if(error==-1) 227 | { 228 | close(Cport[comport_number]); 229 | perror("unable to adjust portsettings "); 230 | return(1); 231 | } 232 | 233 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 234 | { 235 | perror("unable to get portstatus"); 236 | return(1); 237 | } 238 | 239 | status |= TIOCM_DTR; /* turn on DTR */ 240 | status |= TIOCM_RTS; /* turn on RTS */ 241 | 242 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 243 | { 244 | perror("unable to set portstatus"); 245 | return(1); 246 | } 247 | 248 | return(0); 249 | } 250 | 251 | 252 | int RS232_PollComport(int comport_number, unsigned char *buf, int size) 253 | { 254 | int n; 255 | 256 | n = read(Cport[comport_number], buf, size); 257 | 258 | return(n); 259 | } 260 | 261 | 262 | int RS232_SendByte(int comport_number, unsigned char byte) 263 | { 264 | int n; 265 | 266 | n = write(Cport[comport_number], &byte, 1); 267 | if(n<0) return(1); 268 | 269 | return(0); 270 | } 271 | 272 | 273 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size) 274 | { 275 | return(write(Cport[comport_number], buf, size)); 276 | } 277 | 278 | 279 | void RS232_CloseComport(int comport_number) 280 | { 281 | int status; 282 | 283 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 284 | { 285 | perror("unable to get portstatus"); 286 | } 287 | 288 | status &= ~TIOCM_DTR; /* turn off DTR */ 289 | status &= ~TIOCM_RTS; /* turn off RTS */ 290 | 291 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 292 | { 293 | perror("unable to set portstatus"); 294 | } 295 | 296 | tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); 297 | close(Cport[comport_number]); 298 | } 299 | 300 | /* 301 | Constant Description 302 | TIOCM_LE DSR (data set ready/line enable) 303 | TIOCM_DTR DTR (data terminal ready) 304 | TIOCM_RTS RTS (request to send) 305 | TIOCM_ST Secondary TXD (transmit) 306 | TIOCM_SR Secondary RXD (receive) 307 | TIOCM_CTS CTS (clear to send) 308 | TIOCM_CAR DCD (data carrier detect) 309 | TIOCM_CD see TIOCM_CAR 310 | TIOCM_RNG RNG (ring) 311 | TIOCM_RI see TIOCM_RNG 312 | TIOCM_DSR DSR (data set ready) 313 | 314 | http://man7.org/linux/man-pages/man4/tty_ioctl.4.html 315 | */ 316 | 317 | int RS232_IsDCDEnabled(int comport_number) 318 | { 319 | int status; 320 | 321 | ioctl(Cport[comport_number], TIOCMGET, &status); 322 | 323 | if(status&TIOCM_CAR) return(1); 324 | else return(0); 325 | } 326 | 327 | int RS232_IsCTSEnabled(int comport_number) 328 | { 329 | int status; 330 | 331 | ioctl(Cport[comport_number], TIOCMGET, &status); 332 | 333 | if(status&TIOCM_CTS) return(1); 334 | else return(0); 335 | } 336 | 337 | int RS232_IsDSREnabled(int comport_number) 338 | { 339 | int status; 340 | 341 | ioctl(Cport[comport_number], TIOCMGET, &status); 342 | 343 | if(status&TIOCM_DSR) return(1); 344 | else return(0); 345 | } 346 | 347 | void RS232_enableDTR(int comport_number) 348 | { 349 | int status; 350 | 351 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 352 | { 353 | perror("unable to get portstatus"); 354 | } 355 | 356 | status |= TIOCM_DTR; /* turn on DTR */ 357 | 358 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 359 | { 360 | perror("unable to set portstatus"); 361 | } 362 | } 363 | 364 | void RS232_disableDTR(int comport_number) 365 | { 366 | int status; 367 | 368 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 369 | { 370 | perror("unable to get portstatus"); 371 | } 372 | 373 | status &= ~TIOCM_DTR; /* turn off DTR */ 374 | 375 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 376 | { 377 | perror("unable to set portstatus"); 378 | } 379 | } 380 | 381 | void RS232_enableRTS(int comport_number) 382 | { 383 | int status; 384 | 385 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 386 | { 387 | perror("unable to get portstatus"); 388 | } 389 | 390 | status |= TIOCM_RTS; /* turn on RTS */ 391 | 392 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 393 | { 394 | perror("unable to set portstatus"); 395 | } 396 | } 397 | 398 | void RS232_disableRTS(int comport_number) 399 | { 400 | int status; 401 | 402 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 403 | { 404 | perror("unable to get portstatus"); 405 | } 406 | 407 | status &= ~TIOCM_RTS; /* turn off RTS */ 408 | 409 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 410 | { 411 | perror("unable to set portstatus"); 412 | } 413 | } 414 | 415 | 416 | #else /* windows */ 417 | 418 | 419 | HANDLE Cport[16]; 420 | 421 | 422 | char comports[16][10]={"\\\\.\\COM1", "\\\\.\\COM2", "\\\\.\\COM3", "\\\\.\\COM4", 423 | "\\\\.\\COM5", "\\\\.\\COM6", "\\\\.\\COM7", "\\\\.\\COM8", 424 | "\\\\.\\COM9", "\\\\.\\COM10", "\\\\.\\COM11", "\\\\.\\COM12", 425 | "\\\\.\\COM13", "\\\\.\\COM14", "\\\\.\\COM15", "\\\\.\\COM16"}; 426 | 427 | char mode_str[128]; 428 | 429 | 430 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode) 431 | { 432 | if((comport_number>15)||(comport_number<0)) 433 | { 434 | printf("illegal comport number\n"); 435 | return(1); 436 | } 437 | 438 | switch(baudrate) 439 | { 440 | case 110 : strcpy(mode_str, "baud=110"); 441 | break; 442 | case 300 : strcpy(mode_str, "baud=300"); 443 | break; 444 | case 600 : strcpy(mode_str, "baud=600"); 445 | break; 446 | case 1200 : strcpy(mode_str, "baud=1200"); 447 | break; 448 | case 2400 : strcpy(mode_str, "baud=2400"); 449 | break; 450 | case 4800 : strcpy(mode_str, "baud=4800"); 451 | break; 452 | case 9600 : strcpy(mode_str, "baud=9600"); 453 | break; 454 | case 19200 : strcpy(mode_str, "baud=19200"); 455 | break; 456 | case 38400 : strcpy(mode_str, "baud=38400"); 457 | break; 458 | case 57600 : strcpy(mode_str, "baud=57600"); 459 | break; 460 | case 115200 : strcpy(mode_str, "baud=115200"); 461 | break; 462 | case 128000 : strcpy(mode_str, "baud=128000"); 463 | break; 464 | case 256000 : strcpy(mode_str, "baud=256000"); 465 | break; 466 | case 500000 : strcpy(mode_str, "baud=500000"); 467 | break; 468 | case 1000000 : strcpy(mode_str, "baud=1000000"); 469 | break; 470 | default : printf("invalid baudrate\n"); 471 | return(1); 472 | break; 473 | } 474 | 475 | if(strlen(mode) != 3) 476 | { 477 | printf("invalid mode \"%s\"\n", mode); 478 | return(1); 479 | } 480 | 481 | switch(mode[0]) 482 | { 483 | case '8': strcat(mode_str, " data=8"); 484 | break; 485 | case '7': strcat(mode_str, " data=7"); 486 | break; 487 | case '6': strcat(mode_str, " data=6"); 488 | break; 489 | case '5': strcat(mode_str, " data=5"); 490 | break; 491 | default : printf("invalid number of data-bits '%c'\n", mode[0]); 492 | return(1); 493 | break; 494 | } 495 | 496 | switch(mode[1]) 497 | { 498 | case 'N': 499 | case 'n': strcat(mode_str, " parity=n"); 500 | break; 501 | case 'E': 502 | case 'e': strcat(mode_str, " parity=e"); 503 | break; 504 | case 'O': 505 | case 'o': strcat(mode_str, " parity=o"); 506 | break; 507 | default : printf("invalid parity '%c'\n", mode[1]); 508 | return(1); 509 | break; 510 | } 511 | 512 | switch(mode[2]) 513 | { 514 | case '1': strcat(mode_str, " stop=1"); 515 | break; 516 | case '2': strcat(mode_str, " stop=2"); 517 | break; 518 | default : printf("invalid number of stop bits '%c'\n", mode[2]); 519 | return(1); 520 | break; 521 | } 522 | 523 | strcat(mode_str, " dtr=on rts=on"); 524 | 525 | /* 526 | http://msdn.microsoft.com/en-us/library/windows/desktop/aa363145%28v=vs.85%29.aspx 527 | 528 | http://technet.microsoft.com/en-us/library/cc732236.aspx 529 | */ 530 | 531 | Cport[comport_number] = CreateFileA(comports[comport_number], 532 | GENERIC_READ|GENERIC_WRITE, 533 | 0, /* no share */ 534 | NULL, /* no security */ 535 | OPEN_EXISTING, 536 | 0, /* no threads */ 537 | NULL); /* no templates */ 538 | 539 | if(Cport[comport_number]==INVALID_HANDLE_VALUE) 540 | { 541 | printf("unable to open comport\n"); 542 | return(1); 543 | } 544 | 545 | DCB port_settings; 546 | memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */ 547 | port_settings.DCBlength = sizeof(port_settings); 548 | 549 | if(!BuildCommDCBA(mode_str, &port_settings)) 550 | { 551 | printf("unable to set comport dcb settings\n"); 552 | CloseHandle(Cport[comport_number]); 553 | return(1); 554 | } 555 | 556 | if(!SetCommState(Cport[comport_number], &port_settings)) 557 | { 558 | printf("unable to set comport cfg settings\n"); 559 | CloseHandle(Cport[comport_number]); 560 | return(1); 561 | } 562 | 563 | COMMTIMEOUTS Cptimeouts; 564 | 565 | Cptimeouts.ReadIntervalTimeout = MAXDWORD; 566 | Cptimeouts.ReadTotalTimeoutMultiplier = 0; 567 | Cptimeouts.ReadTotalTimeoutConstant = 0; 568 | Cptimeouts.WriteTotalTimeoutMultiplier = 0; 569 | Cptimeouts.WriteTotalTimeoutConstant = 0; 570 | 571 | if(!SetCommTimeouts(Cport[comport_number], &Cptimeouts)) 572 | { 573 | printf("unable to set comport time-out settings\n"); 574 | CloseHandle(Cport[comport_number]); 575 | return(1); 576 | } 577 | 578 | return(0); 579 | } 580 | 581 | 582 | int RS232_PollComport(int comport_number, unsigned char *buf, int size) 583 | { 584 | int n; 585 | 586 | /* added the void pointer cast, otherwise gcc will complain about */ 587 | /* "warning: dereferencing type-punned pointer will break strict aliasing rules" */ 588 | 589 | ReadFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL); 590 | 591 | return(n); 592 | } 593 | 594 | 595 | int RS232_SendByte(int comport_number, unsigned char byte) 596 | { 597 | int n; 598 | 599 | WriteFile(Cport[comport_number], &byte, 1, (LPDWORD)((void *)&n), NULL); 600 | 601 | if(n<0) return(1); 602 | 603 | return(0); 604 | } 605 | 606 | 607 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size) 608 | { 609 | int n; 610 | 611 | if(WriteFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL)) 612 | { 613 | return(n); 614 | } 615 | 616 | return(-1); 617 | } 618 | 619 | 620 | void RS232_CloseComport(int comport_number) 621 | { 622 | CloseHandle(Cport[comport_number]); 623 | } 624 | 625 | /* 626 | http://msdn.microsoft.com/en-us/library/windows/desktop/aa363258%28v=vs.85%29.aspx 627 | */ 628 | 629 | int RS232_IsDCDEnabled(int comport_number) 630 | { 631 | int status; 632 | 633 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 634 | 635 | if(status&MS_RLSD_ON) return(1); 636 | else return(0); 637 | } 638 | 639 | 640 | int RS232_IsCTSEnabled(int comport_number) 641 | { 642 | int status; 643 | 644 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 645 | 646 | if(status&MS_CTS_ON) return(1); 647 | else return(0); 648 | } 649 | 650 | 651 | int RS232_IsDSREnabled(int comport_number) 652 | { 653 | int status; 654 | 655 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 656 | 657 | if(status&MS_DSR_ON) return(1); 658 | else return(0); 659 | } 660 | 661 | 662 | void RS232_enableDTR(int comport_number) 663 | { 664 | EscapeCommFunction(Cport[comport_number], SETDTR); 665 | } 666 | 667 | 668 | void RS232_disableDTR(int comport_number) 669 | { 670 | EscapeCommFunction(Cport[comport_number], CLRDTR); 671 | } 672 | 673 | 674 | void RS232_enableRTS(int comport_number) 675 | { 676 | EscapeCommFunction(Cport[comport_number], SETRTS); 677 | } 678 | 679 | 680 | void RS232_disableRTS(int comport_number) 681 | { 682 | EscapeCommFunction(Cport[comport_number], CLRRTS); 683 | } 684 | 685 | 686 | #endif 687 | 688 | 689 | void RS232_cputs(int comport_number, const char *text) /* sends a string to serial port */ 690 | { 691 | while(*text != 0) RS232_SendByte(comport_number, *(text++)); 692 | } 693 | 694 | 695 | -------------------------------------------------------------------------------- /blurtoothConfig/rs232.c: -------------------------------------------------------------------------------- 1 | /* 2 | *************************************************************************** 3 | * 4 | * Author: Teunis van Beelen 5 | * 6 | * Copyright (C) 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015 Teunis van Beelen 7 | * 8 | * Email: teuniz@gmail.com 9 | * 10 | *************************************************************************** 11 | * 12 | * This program is free software; you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation version 2 of the License. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License along 22 | * with this program; if not, write to the Free Software Foundation, Inc., 23 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 24 | * 25 | *************************************************************************** 26 | * 27 | * This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt 28 | * 29 | *************************************************************************** 30 | */ 31 | 32 | /* Last revision: January 10, 2015 */ 33 | 34 | /* For more info and how to use this library, visit: http://www.teuniz.net/RS-232/ */ 35 | 36 | 37 | #include "rs232.h" 38 | 39 | 40 | 41 | #if defined(__linux__) || defined(__FreeBSD__) /* Linux & FreeBSD */ 42 | 43 | 44 | int Cport[38], 45 | error; 46 | 47 | struct termios new_port_settings, 48 | old_port_settings[38]; 49 | 50 | char comports[38][16]={"/dev/ttyS0","/dev/ttyS1","/dev/ttyS2","/dev/ttyS3","/dev/ttyS4","/dev/ttyS5", 51 | "/dev/ttyS6","/dev/ttyS7","/dev/ttyS8","/dev/ttyS9","/dev/ttyS10","/dev/ttyS11", 52 | "/dev/ttyS12","/dev/ttyS13","/dev/ttyS14","/dev/ttyS15","/dev/ttyUSB0", 53 | "/dev/ttyUSB1","/dev/ttyUSB2","/dev/ttyUSB3","/dev/ttyUSB4","/dev/ttyUSB5", 54 | "/dev/ttyAMA0","/dev/ttyAMA1","/dev/ttyACM0","/dev/ttyACM1", 55 | "/dev/rfcomm0","/dev/rfcomm1","/dev/ircomm0","/dev/ircomm1", 56 | "/dev/cuau0","/dev/cuau1","/dev/cuau2","/dev/cuau3", 57 | "/dev/cuaU0","/dev/cuaU1","/dev/cuaU2","/dev/cuaU3"}; 58 | 59 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode) 60 | { 61 | int baudr, 62 | status; 63 | 64 | if((comport_number>37)||(comport_number<0)) 65 | { 66 | printf("illegal comport number\n"); 67 | return(1); 68 | } 69 | 70 | switch(baudrate) 71 | { 72 | case 50 : baudr = B50; 73 | break; 74 | case 75 : baudr = B75; 75 | break; 76 | case 110 : baudr = B110; 77 | break; 78 | case 134 : baudr = B134; 79 | break; 80 | case 150 : baudr = B150; 81 | break; 82 | case 200 : baudr = B200; 83 | break; 84 | case 300 : baudr = B300; 85 | break; 86 | case 600 : baudr = B600; 87 | break; 88 | case 1200 : baudr = B1200; 89 | break; 90 | case 1800 : baudr = B1800; 91 | break; 92 | case 2400 : baudr = B2400; 93 | break; 94 | case 4800 : baudr = B4800; 95 | break; 96 | case 9600 : baudr = B9600; 97 | break; 98 | case 19200 : baudr = B19200; 99 | break; 100 | case 38400 : baudr = B38400; 101 | break; 102 | case 57600 : baudr = B57600; 103 | break; 104 | case 115200 : baudr = B115200; 105 | break; 106 | case 230400 : baudr = B230400; 107 | break; 108 | case 460800 : baudr = B460800; 109 | break; 110 | case 500000 : baudr = B500000; 111 | break; 112 | case 576000 : baudr = B576000; 113 | break; 114 | case 921600 : baudr = B921600; 115 | break; 116 | case 1000000 : baudr = B1000000; 117 | break; 118 | case 1152000 : baudr = B1152000; 119 | break; 120 | case 1500000 : baudr = B1500000; 121 | break; 122 | case 2000000 : baudr = B2000000; 123 | break; 124 | case 2500000 : baudr = B2500000; 125 | break; 126 | case 3000000 : baudr = B3000000; 127 | break; 128 | case 3500000 : baudr = B3500000; 129 | break; 130 | case 4000000 : baudr = B4000000; 131 | break; 132 | default : printf("invalid baudrate\n"); 133 | return(1); 134 | break; 135 | } 136 | 137 | int cbits=CS8, 138 | cpar=0, 139 | ipar=IGNPAR, 140 | bstop=0; 141 | 142 | if(strlen(mode) != 3) 143 | { 144 | printf("invalid mode \"%s\"\n", mode); 145 | return(1); 146 | } 147 | 148 | switch(mode[0]) 149 | { 150 | case '8': cbits = CS8; 151 | break; 152 | case '7': cbits = CS7; 153 | break; 154 | case '6': cbits = CS6; 155 | break; 156 | case '5': cbits = CS5; 157 | break; 158 | default : printf("invalid number of data-bits '%c'\n", mode[0]); 159 | return(1); 160 | break; 161 | } 162 | 163 | switch(mode[1]) 164 | { 165 | case 'N': 166 | case 'n': cpar = 0; 167 | ipar = IGNPAR; 168 | break; 169 | case 'E': 170 | case 'e': cpar = PARENB; 171 | ipar = INPCK; 172 | break; 173 | case 'O': 174 | case 'o': cpar = (PARENB | PARODD); 175 | ipar = INPCK; 176 | break; 177 | default : printf("invalid parity '%c'\n", mode[1]); 178 | return(1); 179 | break; 180 | } 181 | 182 | switch(mode[2]) 183 | { 184 | case '1': bstop = 0; 185 | break; 186 | case '2': bstop = CSTOPB; 187 | break; 188 | default : printf("invalid number of stop bits '%c'\n", mode[2]); 189 | return(1); 190 | break; 191 | } 192 | 193 | /* 194 | http://pubs.opengroup.org/onlinepubs/7908799/xsh/termios.h.html 195 | 196 | http://man7.org/linux/man-pages/man3/termios.3.html 197 | */ 198 | 199 | Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY); 200 | if(Cport[comport_number]==-1) 201 | { 202 | perror("unable to open comport "); 203 | return(1); 204 | } 205 | 206 | error = tcgetattr(Cport[comport_number], old_port_settings + comport_number); 207 | if(error==-1) 208 | { 209 | close(Cport[comport_number]); 210 | perror("unable to read portsettings "); 211 | return(1); 212 | } 213 | memset(&new_port_settings, 0, sizeof(new_port_settings)); /* clear the new struct */ 214 | 215 | new_port_settings.c_cflag = cbits | cpar | bstop | CLOCAL | CREAD; 216 | new_port_settings.c_iflag = ipar; 217 | new_port_settings.c_oflag = 0; 218 | new_port_settings.c_lflag = 0; 219 | new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ 220 | new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ 221 | 222 | cfsetispeed(&new_port_settings, baudr); 223 | cfsetospeed(&new_port_settings, baudr); 224 | 225 | error = tcsetattr(Cport[comport_number], TCSANOW, &new_port_settings); 226 | if(error==-1) 227 | { 228 | close(Cport[comport_number]); 229 | perror("unable to adjust portsettings "); 230 | return(1); 231 | } 232 | 233 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 234 | { 235 | perror("unable to get portstatus"); 236 | return(1); 237 | } 238 | 239 | status |= TIOCM_DTR; /* turn on DTR */ 240 | status |= TIOCM_RTS; /* turn on RTS */ 241 | 242 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 243 | { 244 | perror("unable to set portstatus"); 245 | return(1); 246 | } 247 | 248 | return(0); 249 | } 250 | 251 | 252 | int RS232_PollComport(int comport_number, unsigned char *buf, int size) 253 | { 254 | int n; 255 | 256 | n = read(Cport[comport_number], buf, size); 257 | 258 | return(n); 259 | } 260 | 261 | 262 | int RS232_SendByte(int comport_number, unsigned char byte) 263 | { 264 | int n; 265 | 266 | n = write(Cport[comport_number], &byte, 1); 267 | if(n<0) return(1); 268 | 269 | return(0); 270 | } 271 | 272 | 273 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size) 274 | { 275 | return(write(Cport[comport_number], buf, size)); 276 | } 277 | 278 | 279 | void RS232_CloseComport(int comport_number) 280 | { 281 | int status; 282 | 283 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 284 | { 285 | perror("unable to get portstatus"); 286 | } 287 | 288 | status &= ~TIOCM_DTR; /* turn off DTR */ 289 | status &= ~TIOCM_RTS; /* turn off RTS */ 290 | 291 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 292 | { 293 | perror("unable to set portstatus"); 294 | } 295 | 296 | tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number); 297 | close(Cport[comport_number]); 298 | } 299 | 300 | /* 301 | Constant Description 302 | TIOCM_LE DSR (data set ready/line enable) 303 | TIOCM_DTR DTR (data terminal ready) 304 | TIOCM_RTS RTS (request to send) 305 | TIOCM_ST Secondary TXD (transmit) 306 | TIOCM_SR Secondary RXD (receive) 307 | TIOCM_CTS CTS (clear to send) 308 | TIOCM_CAR DCD (data carrier detect) 309 | TIOCM_CD see TIOCM_CAR 310 | TIOCM_RNG RNG (ring) 311 | TIOCM_RI see TIOCM_RNG 312 | TIOCM_DSR DSR (data set ready) 313 | 314 | http://man7.org/linux/man-pages/man4/tty_ioctl.4.html 315 | */ 316 | 317 | int RS232_IsDCDEnabled(int comport_number) 318 | { 319 | int status; 320 | 321 | ioctl(Cport[comport_number], TIOCMGET, &status); 322 | 323 | if(status&TIOCM_CAR) return(1); 324 | else return(0); 325 | } 326 | 327 | int RS232_IsCTSEnabled(int comport_number) 328 | { 329 | int status; 330 | 331 | ioctl(Cport[comport_number], TIOCMGET, &status); 332 | 333 | if(status&TIOCM_CTS) return(1); 334 | else return(0); 335 | } 336 | 337 | int RS232_IsDSREnabled(int comport_number) 338 | { 339 | int status; 340 | 341 | ioctl(Cport[comport_number], TIOCMGET, &status); 342 | 343 | if(status&TIOCM_DSR) return(1); 344 | else return(0); 345 | } 346 | 347 | void RS232_enableDTR(int comport_number) 348 | { 349 | int status; 350 | 351 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 352 | { 353 | perror("unable to get portstatus"); 354 | } 355 | 356 | status |= TIOCM_DTR; /* turn on DTR */ 357 | 358 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 359 | { 360 | perror("unable to set portstatus"); 361 | } 362 | } 363 | 364 | void RS232_disableDTR(int comport_number) 365 | { 366 | int status; 367 | 368 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 369 | { 370 | perror("unable to get portstatus"); 371 | } 372 | 373 | status &= ~TIOCM_DTR; /* turn off DTR */ 374 | 375 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 376 | { 377 | perror("unable to set portstatus"); 378 | } 379 | } 380 | 381 | void RS232_enableRTS(int comport_number) 382 | { 383 | int status; 384 | 385 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 386 | { 387 | perror("unable to get portstatus"); 388 | } 389 | 390 | status |= TIOCM_RTS; /* turn on RTS */ 391 | 392 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 393 | { 394 | perror("unable to set portstatus"); 395 | } 396 | } 397 | 398 | void RS232_disableRTS(int comport_number) 399 | { 400 | int status; 401 | 402 | if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1) 403 | { 404 | perror("unable to get portstatus"); 405 | } 406 | 407 | status &= ~TIOCM_RTS; /* turn off RTS */ 408 | 409 | if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1) 410 | { 411 | perror("unable to set portstatus"); 412 | } 413 | } 414 | 415 | 416 | #else /* windows */ 417 | 418 | 419 | HANDLE Cport[16]; 420 | 421 | 422 | char comports[16][10]={"\\\\.\\COM1", "\\\\.\\COM2", "\\\\.\\COM3", "\\\\.\\COM4", 423 | "\\\\.\\COM5", "\\\\.\\COM6", "\\\\.\\COM7", "\\\\.\\COM8", 424 | "\\\\.\\COM9", "\\\\.\\COM10", "\\\\.\\COM11", "\\\\.\\COM12", 425 | "\\\\.\\COM13", "\\\\.\\COM14", "\\\\.\\COM15", "\\\\.\\COM16"}; 426 | 427 | char mode_str[128]; 428 | 429 | 430 | int RS232_OpenComport(int comport_number, int baudrate, const char *mode) 431 | { 432 | if((comport_number>15)||(comport_number<0)) 433 | { 434 | printf("illegal comport number\n"); 435 | return(1); 436 | } 437 | 438 | switch(baudrate) 439 | { 440 | case 110 : strcpy(mode_str, "baud=110"); 441 | break; 442 | case 300 : strcpy(mode_str, "baud=300"); 443 | break; 444 | case 600 : strcpy(mode_str, "baud=600"); 445 | break; 446 | case 1200 : strcpy(mode_str, "baud=1200"); 447 | break; 448 | case 2400 : strcpy(mode_str, "baud=2400"); 449 | break; 450 | case 4800 : strcpy(mode_str, "baud=4800"); 451 | break; 452 | case 9600 : strcpy(mode_str, "baud=9600"); 453 | break; 454 | case 19200 : strcpy(mode_str, "baud=19200"); 455 | break; 456 | case 38400 : strcpy(mode_str, "baud=38400"); 457 | break; 458 | case 57600 : strcpy(mode_str, "baud=57600"); 459 | break; 460 | case 115200 : strcpy(mode_str, "baud=115200"); 461 | break; 462 | case 128000 : strcpy(mode_str, "baud=128000"); 463 | break; 464 | case 256000 : strcpy(mode_str, "baud=256000"); 465 | break; 466 | case 500000 : strcpy(mode_str, "baud=500000"); 467 | break; 468 | case 1000000 : strcpy(mode_str, "baud=1000000"); 469 | break; 470 | default : printf("invalid baudrate\n"); 471 | return(1); 472 | break; 473 | } 474 | 475 | if(strlen(mode) != 3) 476 | { 477 | printf("invalid mode \"%s\"\n", mode); 478 | return(1); 479 | } 480 | 481 | switch(mode[0]) 482 | { 483 | case '8': strcat(mode_str, " data=8"); 484 | break; 485 | case '7': strcat(mode_str, " data=7"); 486 | break; 487 | case '6': strcat(mode_str, " data=6"); 488 | break; 489 | case '5': strcat(mode_str, " data=5"); 490 | break; 491 | default : printf("invalid number of data-bits '%c'\n", mode[0]); 492 | return(1); 493 | break; 494 | } 495 | 496 | switch(mode[1]) 497 | { 498 | case 'N': 499 | case 'n': strcat(mode_str, " parity=n"); 500 | break; 501 | case 'E': 502 | case 'e': strcat(mode_str, " parity=e"); 503 | break; 504 | case 'O': 505 | case 'o': strcat(mode_str, " parity=o"); 506 | break; 507 | default : printf("invalid parity '%c'\n", mode[1]); 508 | return(1); 509 | break; 510 | } 511 | 512 | switch(mode[2]) 513 | { 514 | case '1': strcat(mode_str, " stop=1"); 515 | break; 516 | case '2': strcat(mode_str, " stop=2"); 517 | break; 518 | default : printf("invalid number of stop bits '%c'\n", mode[2]); 519 | return(1); 520 | break; 521 | } 522 | 523 | strcat(mode_str, " dtr=on rts=on"); 524 | 525 | /* 526 | http://msdn.microsoft.com/en-us/library/windows/desktop/aa363145%28v=vs.85%29.aspx 527 | 528 | http://technet.microsoft.com/en-us/library/cc732236.aspx 529 | */ 530 | 531 | Cport[comport_number] = CreateFileA(comports[comport_number], 532 | GENERIC_READ|GENERIC_WRITE, 533 | 0, /* no share */ 534 | NULL, /* no security */ 535 | OPEN_EXISTING, 536 | 0, /* no threads */ 537 | NULL); /* no templates */ 538 | 539 | if(Cport[comport_number]==INVALID_HANDLE_VALUE) 540 | { 541 | printf("unable to open comport\n"); 542 | return(1); 543 | } 544 | 545 | DCB port_settings; 546 | memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */ 547 | port_settings.DCBlength = sizeof(port_settings); 548 | 549 | if(!BuildCommDCBA(mode_str, &port_settings)) 550 | { 551 | printf("unable to set comport dcb settings\n"); 552 | CloseHandle(Cport[comport_number]); 553 | return(1); 554 | } 555 | 556 | if(!SetCommState(Cport[comport_number], &port_settings)) 557 | { 558 | printf("unable to set comport cfg settings\n"); 559 | CloseHandle(Cport[comport_number]); 560 | return(1); 561 | } 562 | 563 | COMMTIMEOUTS Cptimeouts; 564 | 565 | Cptimeouts.ReadIntervalTimeout = MAXDWORD; 566 | Cptimeouts.ReadTotalTimeoutMultiplier = 0; 567 | Cptimeouts.ReadTotalTimeoutConstant = 0; 568 | Cptimeouts.WriteTotalTimeoutMultiplier = 0; 569 | Cptimeouts.WriteTotalTimeoutConstant = 0; 570 | 571 | if(!SetCommTimeouts(Cport[comport_number], &Cptimeouts)) 572 | { 573 | printf("unable to set comport time-out settings\n"); 574 | CloseHandle(Cport[comport_number]); 575 | return(1); 576 | } 577 | 578 | return(0); 579 | } 580 | 581 | 582 | int RS232_PollComport(int comport_number, unsigned char *buf, int size) 583 | { 584 | int n; 585 | 586 | /* added the void pointer cast, otherwise gcc will complain about */ 587 | /* "warning: dereferencing type-punned pointer will break strict aliasing rules" */ 588 | 589 | ReadFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL); 590 | 591 | return(n); 592 | } 593 | 594 | 595 | int RS232_SendByte(int comport_number, unsigned char byte) 596 | { 597 | int n; 598 | 599 | WriteFile(Cport[comport_number], &byte, 1, (LPDWORD)((void *)&n), NULL); 600 | 601 | if(n<0) return(1); 602 | 603 | return(0); 604 | } 605 | 606 | 607 | int RS232_SendBuf(int comport_number, unsigned char *buf, int size) 608 | { 609 | int n; 610 | 611 | if(WriteFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL)) 612 | { 613 | return(n); 614 | } 615 | 616 | return(-1); 617 | } 618 | 619 | 620 | void RS232_CloseComport(int comport_number) 621 | { 622 | CloseHandle(Cport[comport_number]); 623 | } 624 | 625 | /* 626 | http://msdn.microsoft.com/en-us/library/windows/desktop/aa363258%28v=vs.85%29.aspx 627 | */ 628 | 629 | int RS232_IsDCDEnabled(int comport_number) 630 | { 631 | int status; 632 | 633 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 634 | 635 | if(status&MS_RLSD_ON) return(1); 636 | else return(0); 637 | } 638 | 639 | 640 | int RS232_IsCTSEnabled(int comport_number) 641 | { 642 | int status; 643 | 644 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 645 | 646 | if(status&MS_CTS_ON) return(1); 647 | else return(0); 648 | } 649 | 650 | 651 | int RS232_IsDSREnabled(int comport_number) 652 | { 653 | int status; 654 | 655 | GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status)); 656 | 657 | if(status&MS_DSR_ON) return(1); 658 | else return(0); 659 | } 660 | 661 | 662 | void RS232_enableDTR(int comport_number) 663 | { 664 | EscapeCommFunction(Cport[comport_number], SETDTR); 665 | } 666 | 667 | 668 | void RS232_disableDTR(int comport_number) 669 | { 670 | EscapeCommFunction(Cport[comport_number], CLRDTR); 671 | } 672 | 673 | 674 | void RS232_enableRTS(int comport_number) 675 | { 676 | EscapeCommFunction(Cport[comport_number], SETRTS); 677 | } 678 | 679 | 680 | void RS232_disableRTS(int comport_number) 681 | { 682 | EscapeCommFunction(Cport[comport_number], CLRRTS); 683 | } 684 | 685 | 686 | #endif 687 | 688 | 689 | void RS232_cputs(int comport_number, const char *text) /* sends a string to serial port */ 690 | { 691 | while(*text != 0) RS232_SendByte(comport_number, *(text++)); 692 | } 693 | 694 | 695 | -------------------------------------------------------------------------------- /newtonMethod/algorithm_matrix.c: -------------------------------------------------------------------------------- 1 | /*====================================================================================================*/ 2 | /*====================================================================================================*/ 3 | #include 4 | #include 5 | #include 6 | 7 | #include "algorithm_matrix.h" 8 | /*====================================================================================================*/ 9 | /*====================================================================================================*/ 10 | static void MATRIX_ERROR( void ) 11 | { 12 | printf("\nERROR...\n"); 13 | while(1); 14 | } 15 | /*====================================================================================================*/ 16 | /*====================================================================================================* 17 | **函數 : Matrix_init 18 | **功能 : Init Matrix 19 | **輸入 : *pMatrix, *pArray, rows, cols 20 | **輸出 : none 21 | **使用 : Matrix_init(matrix, array, rows, cols); 22 | **====================================================================================================*/ 23 | /*====================================================================================================*/ 24 | void Matrix_init( Matrix_TypeDef *pMatrix, double *pArray, uint16_t rows, uint16_t cols ) 25 | { 26 | pMatrix->rows = rows; 27 | pMatrix->cols = cols; 28 | pMatrix->total = rows * cols; 29 | pMatrix->arr = pArray; 30 | } 31 | /*====================================================================================================*/ 32 | /*====================================================================================================* 33 | **函數 : Matrix_clear 34 | **功能 : Clear Matrix 35 | **輸入 : *pMatrix 36 | **輸出 : none 37 | **使用 : Matrix_clear(matrix); 38 | **====================================================================================================*/ 39 | /*====================================================================================================*/ 40 | void Matrix_clear( Matrix_TypeDef *pMatrix ) 41 | { 42 | memset(pMatrix->arr, 0, sizeof(double) * pMatrix->total); 43 | } 44 | /*====================================================================================================*/ 45 | /*====================================================================================================* 46 | **函數 : Matrix_create 47 | **功能 : Create Matrix 48 | **輸入 : rows, cols 49 | **輸出 : *matrix 50 | **使用 : matrix = Matrix_create(rows, cols); 51 | **====================================================================================================*/ 52 | /*====================================================================================================*/ 53 | Matrix_TypeDef *Matrix_create( uint16_t rows, uint16_t cols ) 54 | { 55 | Matrix_TypeDef *matrix; 56 | 57 | matrix = (Matrix_TypeDef *)malloc(sizeof(Matrix_TypeDef)); 58 | matrix->mState = MSTATE_MALLOC; 59 | matrix->rows = rows; 60 | matrix->cols = cols; 61 | matrix->total = rows * cols; 62 | matrix->arr = (double *)malloc(sizeof(double) * matrix->total); 63 | Matrix_clear(matrix); 64 | 65 | return matrix; 66 | } 67 | /*====================================================================================================*/ 68 | /*====================================================================================================* 69 | **函數 : Matrix_create 70 | **功能 : Create Matrix 71 | **輸入 : *pArray, rows, cols 72 | **輸出 : *matrix 73 | **使用 : matrix = Matrix_createPtr(array, rows, cols); 74 | **====================================================================================================*/ 75 | /*====================================================================================================*/ 76 | Matrix_TypeDef *Matrix_createPtr( double *pArray, uint16_t rows, uint16_t cols ) 77 | { 78 | Matrix_TypeDef *matrix; 79 | 80 | matrix = (Matrix_TypeDef *)malloc(sizeof(Matrix_TypeDef)); 81 | matrix->mState = MSTATE_POINTER; 82 | matrix->rows = rows; 83 | matrix->cols = cols; 84 | matrix->total = rows * cols; 85 | matrix->arr = pArray; 86 | 87 | return matrix; 88 | } 89 | /*====================================================================================================*/ 90 | /*====================================================================================================* 91 | **函數 : Matrix_delete 92 | **功能 : Delete Matrix 93 | **輸入 : *matrix 94 | **輸出 : none 95 | **使用 : Matrix_delete(matrix); 96 | **====================================================================================================*/ 97 | /*====================================================================================================*/ 98 | void Matrix_delete( Matrix_TypeDef *matrix ) 99 | { 100 | if(matrix->mState == MSTATE_MALLOC) { 101 | free(matrix->arr); 102 | } 103 | free(matrix); 104 | } 105 | /*====================================================================================================*/ 106 | /*====================================================================================================* 107 | **函數 : Matrix_createDiag 108 | **功能 : Create Diagonal Matrix 109 | **輸入 : cols, data 110 | **輸出 : *matrix 111 | **使用 : matrix = Matrix_createDiag(cols, data); 112 | **====================================================================================================*/ 113 | /*====================================================================================================*/ 114 | Matrix_TypeDef *Matrix_createDiag( uint16_t cols, double data ) 115 | { 116 | Matrix_TypeDef *matrix = Matrix_create(cols, cols); 117 | 118 | for(uint16_t i = 0; i < cols; i++) 119 | matrix->arr[i * matrix->cols + i] = data; 120 | 121 | return matrix; 122 | } 123 | /*====================================================================================================*/ 124 | /*====================================================================================================* 125 | **函數 : Matrix_copy 126 | **功能 : Copy Matrix 127 | **輸入 : *pMatrixC1, *pMatrixC2 128 | **輸出 : none 129 | **使用 : Matrix_copy(matrixC1, matrixC2); 130 | **====================================================================================================*/ 131 | /*====================================================================================================*/ 132 | void Matrix_copy( Matrix_TypeDef *pMatrixC1, Matrix_TypeDef *pMatrixC2 ) 133 | { 134 | if((pMatrixC1->rows != pMatrixC2->rows) || (pMatrixC1->cols != pMatrixC2->cols)) 135 | MATRIX_ERROR(); 136 | 137 | for(uint32_t i = 0; i < pMatrixC1->total; i++) 138 | pMatrixC1->arr[i] = pMatrixC2->arr[i]; 139 | } 140 | /*====================================================================================================*/ 141 | /*====================================================================================================* 142 | **函數 : Matrix_copyMatrix 143 | **功能 : Copy Matrix 144 | **輸入 : *pMatrix 145 | **輸出 : *matrix 146 | **使用 : matrix = Matrix_copyMatrix(matrixCpy); 147 | **====================================================================================================*/ 148 | /*====================================================================================================*/ 149 | Matrix_TypeDef *Matrix_copyMatrix( Matrix_TypeDef *pMatrix ) 150 | { 151 | Matrix_TypeDef *matrix = Matrix_create(pMatrix->rows, pMatrix->cols); 152 | 153 | for(uint32_t i = 0; i < matrix->total; i++) 154 | matrix->arr[i] = pMatrix->arr[i]; 155 | 156 | return matrix; 157 | } 158 | /*====================================================================================================*/ 159 | /*====================================================================================================* 160 | **函數 : Matrix_copyArr 161 | **功能 : Copy Array to Matrix 162 | **輸入 : *pArray, rows, cols 163 | **輸出 : *matrix 164 | **使用 : matrix = Matrix_copyArray(array, row, col); 165 | **====================================================================================================*/ 166 | /*====================================================================================================*/ 167 | Matrix_TypeDef *Matrix_copyArray( double *pArray, uint16_t rows, uint16_t cols ) 168 | { 169 | Matrix_TypeDef *matrix = Matrix_create(rows, cols); 170 | 171 | for(uint32_t i = 0; i < matrix->total; i++) 172 | matrix->arr[i] = pArray[i]; 173 | 174 | return matrix; 175 | } 176 | /*====================================================================================================*/ 177 | /*====================================================================================================* 178 | **函數 : Matrix_resize 179 | **功能 : Resize Matrix 180 | **輸入 : *pMatrix, rows, cols 181 | **輸出 : none 182 | **使用 : Matrix_resize(matrix, row, col); 183 | **====================================================================================================*/ 184 | /*====================================================================================================*/ 185 | void Matrix_resize( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols ) 186 | { 187 | /* 188 | Matrix_TypeDef *matrix = Matrix_create(rows, cols); 189 | 190 | for(uint32_t i = 0; i < matrix->total; i++) 191 | matrix->arr[i] = array[i]; 192 | 193 | return matrix; 194 | */ 195 | } 196 | /*====================================================================================================*/ 197 | /*====================================================================================================* 198 | **函數 : Matrix_setData 199 | **功能 : Set Data 200 | **輸入 : *pMatrix, rows, cols, data 201 | **輸出 : None 202 | **使用 : Matrix_setData(matrix, rows, cols, data); 203 | **====================================================================================================*/ 204 | /*====================================================================================================*/ 205 | void Matrix_setData( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols, double data ) 206 | { 207 | if((rows < pMatrix->rows) && (cols < pMatrix->cols)) 208 | MATRIX_ERROR(); 209 | 210 | pMatrix->arr[rows * pMatrix->cols + cols] = data; 211 | } 212 | /*====================================================================================================*/ 213 | /*====================================================================================================* 214 | **函數 : Matrix_getData 215 | **功能 : Get Data 216 | **輸入 : *pMatrix, rows, cols 217 | **輸出 : data 218 | **使用 : data = Matrix_getData(matrix, rows, cols); 219 | **====================================================================================================*/ 220 | /*====================================================================================================*/ 221 | double Matrix_getData( Matrix_TypeDef *pMatrix, uint16_t rows, uint16_t cols ) 222 | { 223 | if((rows < pMatrix->rows) && (cols < pMatrix->cols)) 224 | MATRIX_ERROR(); 225 | 226 | return pMatrix->arr[rows * pMatrix->cols + cols]; 227 | } 228 | /*====================================================================================================*/ 229 | /*====================================================================================================* 230 | **函數 : Matrix_setDiag 231 | **功能 : Set Diagonal Matrix 232 | **輸入 : *pMatrix, data 233 | **輸出 : none 234 | **使用 : Matrix_setDiag(matrix, data); 235 | **====================================================================================================*/ 236 | /*====================================================================================================*/ 237 | void Matrix_setDiag( Matrix_TypeDef *pMatrix, double data ) 238 | { 239 | if(pMatrix->rows != pMatrix->cols) 240 | MATRIX_ERROR(); 241 | 242 | for(uint16_t i = 0; i < pMatrix->cols; i++) 243 | pMatrix->arr[i * pMatrix->cols + i] = data; 244 | } 245 | /*====================================================================================================*/ 246 | /*====================================================================================================* 247 | **函數 : Matrix_getDiag 248 | **功能 : Get Diagonal 249 | **輸入 : *pMatrix, pMatrixD 250 | **輸出 : none 251 | **使用 : Matrix_getDiag(matrix, matrixD); 252 | **====================================================================================================*/ 253 | /*====================================================================================================*/ 254 | void Matrix_getDiag( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixD ) 255 | { 256 | if((pMatrix->rows != pMatrix->cols) || (pMatrixD->rows != pMatrixD->cols) || (pMatrix->rows != pMatrixD->cols)) 257 | MATRIX_ERROR(); 258 | 259 | if(pMatrix != pMatrixD) { 260 | Matrix_clear(pMatrix); 261 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 262 | pMatrix->arr[i * pMatrix->cols + i] = pMatrixD->arr[i * pMatrixD->rows + i]; 263 | } 264 | } 265 | else { 266 | Matrix_TypeDef *matrix = Matrix_copyMatrix(pMatrixD); 267 | for(uint16_t i = 0; i < pMatrix->rows; i++) 268 | pMatrix->arr[i * pMatrix->cols + i] = matrix->arr[i * matrix->rows + i]; 269 | } 270 | } 271 | /*====================================================================================================*/ 272 | /*====================================================================================================* 273 | **函數 : Matrix_setMatrix 274 | **功能 : Set Matrix 275 | **輸入 : *pMatrix, *pMatrixS, rows_pos, cols_pos 276 | **輸出 : none 277 | **使用 : Matrix_setMatrix(matrix, matrixS, rows_pos, cols_pos); 278 | **====================================================================================================*/ 279 | /*====================================================================================================*/ 280 | void Matrix_setMatrix( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixS, uint16_t rows_pos, uint16_t cols_pos ) 281 | { 282 | if(((rows_pos + pMatrixS->rows) > pMatrix->rows) || ((cols_pos + pMatrixS->cols) > pMatrix->cols)) 283 | MATRIX_ERROR(); 284 | 285 | uint32_t cnt = 0; 286 | for(uint16_t i = rows_pos; i < rows_pos + pMatrixS->rows; i++) { 287 | for(uint16_t j = cols_pos; j < cols_pos + pMatrixS->cols; j++) { 288 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixS->arr[cnt++]; 289 | } 290 | } 291 | } 292 | /*====================================================================================================*/ 293 | /*====================================================================================================* 294 | **函數 : Matrix_getMatrix 295 | **功能 : Get Matrix 296 | **輸入 : *pMatrixG, *pMatrix, rows_pos, cols_pos 297 | **輸出 : none 298 | **使用 : Matrix_getMatrix(matrixG, matrix, rows_pos, cols_pos); 299 | **====================================================================================================*/ 300 | /*====================================================================================================*/ 301 | void Matrix_getMatrix( Matrix_TypeDef *pMatrixG, Matrix_TypeDef *pMatrix, uint16_t rows_pos, uint16_t cols_pos ) 302 | { 303 | if(((rows_pos + pMatrixG->rows) > pMatrix->rows) || ((cols_pos + pMatrixG->cols) > pMatrix->cols)) 304 | MATRIX_ERROR(); 305 | 306 | uint32_t cnt = 0; 307 | for(uint16_t i = rows_pos; i < rows_pos + pMatrixG->rows; i++) { 308 | for(uint16_t j = cols_pos; j < cols_pos + pMatrixG->cols; j++) { 309 | pMatrixG->arr[cnt++] = pMatrix->arr[i * pMatrix->cols + j]; 310 | } 311 | } 312 | } 313 | /*====================================================================================================*/ 314 | /*====================================================================================================* 315 | **函數 : Matrix_add 316 | **功能 : Matrix Addition 317 | **輸入 : *pMatrix, *pMatrixA1, pMatrixA2 318 | **輸出 : none 319 | **使用 : Matrix_add(matrix, matrixA1, matrixA2); 320 | **====================================================================================================*/ 321 | /*====================================================================================================*/ 322 | void Matrix_add( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixA1, Matrix_TypeDef *pMatrixA2 ) 323 | { 324 | if((pMatrix->rows != pMatrixA1->rows) || (pMatrix->cols != pMatrixA1->cols) || 325 | (pMatrix->rows != pMatrixA2->rows) || (pMatrix->cols != pMatrixA2->cols)) 326 | MATRIX_ERROR(); 327 | 328 | for(uint16_t i = 0; i < pMatrix->rows; i++) 329 | for(uint16_t j = 0; j < pMatrix->cols; j++) 330 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixA1->arr[i * pMatrixA1->cols + j] + pMatrixA2->arr[i * pMatrixA2->cols + j]; 331 | } 332 | /*====================================================================================================*/ 333 | /*====================================================================================================* 334 | **函數 : Matrix_sub 335 | **功能 : Matrix Subtraction 336 | **輸入 : *pMatrix, *pMatrixS1, *pMatrixS2 337 | **輸出 : none 338 | **使用 : Matrix_sub(matrix, matrixS1, matrixS2); 339 | **====================================================================================================*/ 340 | /*====================================================================================================*/ 341 | void Matrix_sub( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixS1, Matrix_TypeDef *pMatrixS2 ) 342 | { 343 | if((pMatrix->rows != pMatrixS1->rows) || (pMatrix->cols != pMatrixS1->cols) || 344 | (pMatrix->rows != pMatrixS2->rows) || (pMatrix->cols != pMatrixS2->cols)) 345 | MATRIX_ERROR(); 346 | 347 | for(uint16_t i = 0; i < pMatrix->rows; i++) 348 | for(uint16_t j = 0; j < pMatrix->cols; j++) 349 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixS1->arr[i * pMatrixS1->cols + j] - pMatrixS2->arr[i * pMatrixS2->cols + j]; 350 | } 351 | /*====================================================================================================*/ 352 | /*====================================================================================================* 353 | **函數 : Matrix_mul 354 | **功能 : Matrix Multiplication 355 | **輸入 : *pMatrix, *pMatrixM1, *pMatrixM2 356 | **輸出 : none 357 | **使用 : Matrix_mul(matrix, matrixM1, matrixM2); 358 | **====================================================================================================*/ 359 | /*====================================================================================================*/ 360 | void Matrix_mul( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixM1, Matrix_TypeDef *pMatrixM2 ) 361 | { 362 | if((pMatrixM1->cols != pMatrixM2->rows) || (pMatrix->rows != pMatrixM1->rows) || (pMatrix->cols != pMatrixM2->cols)) 363 | MATRIX_ERROR(); 364 | 365 | if(pMatrix == pMatrixM1) { 366 | Matrix_TypeDef *matrix = Matrix_copyMatrix(pMatrixM1); 367 | Matrix_clear(pMatrix); 368 | for(uint16_t i = 0; i < matrix->rows; i++) { 369 | for(uint16_t j = 0; j < pMatrixM2->cols; j++) { 370 | for(uint16_t k = 0; k < matrix->cols; k++) { 371 | pMatrix->arr[i * pMatrix->cols + j] += matrix->arr[i * matrix->cols + k] * pMatrixM2->arr[k * pMatrixM2->cols + j]; 372 | } 373 | } 374 | } 375 | } 376 | else if(pMatrix == pMatrixM2) { 377 | Matrix_TypeDef *matrix = Matrix_copyMatrix(pMatrixM2); 378 | Matrix_clear(pMatrix); 379 | for(uint16_t i = 0; i < pMatrixM1->rows; i++) { 380 | for(uint16_t j = 0; j < matrix->cols; j++) { 381 | for(uint16_t k = 0; k < pMatrixM1->cols; k++) { 382 | pMatrix->arr[i * pMatrix->cols + j] += pMatrixM1->arr[i * pMatrixM1->cols + k] * matrix->arr[k * matrix->cols + j]; 383 | } 384 | } 385 | } 386 | } 387 | else { 388 | Matrix_clear(pMatrix); 389 | for(uint16_t i = 0; i < pMatrixM1->rows; i++) { 390 | for(uint16_t j = 0; j < pMatrixM2->cols; j++) { 391 | for(uint16_t k = 0; k < pMatrixM1->cols; k++) { 392 | pMatrix->arr[i * pMatrix->cols + j] += pMatrixM1->arr[i * pMatrixM1->cols + k] * pMatrixM2->arr[k * pMatrixM2->cols + j]; 393 | } 394 | } 395 | } 396 | } 397 | } 398 | /*====================================================================================================*/ 399 | /*====================================================================================================* 400 | **函數 : Matrix_mulNumb 401 | **功能 : Matrix Multiplication 402 | **輸入 : *pMatrix, *pMatrixM1, number 403 | **輸出 : none 404 | **使用 : Matrix_mulNumb(matrix, matrixM1, number); 405 | **====================================================================================================*/ 406 | /*====================================================================================================*/ 407 | void Matrix_mulNumb( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixM1, double number ) 408 | { 409 | if((pMatrix->cols != pMatrixM1->cols) || (pMatrix->rows != pMatrixM1->rows)) 410 | MATRIX_ERROR(); 411 | 412 | for(uint32_t i = 0; i < pMatrix->total; i++) { 413 | pMatrix->arr[i] = pMatrixM1->arr[i] * number; 414 | } 415 | } 416 | /*====================================================================================================*/ 417 | /*====================================================================================================* 418 | **函數 : Matrix_transpose 419 | **功能 : Matrix Transpose 420 | **輸入 : *pMatrix, *pMatrixT 421 | **輸出 : none 422 | **使用 : Matrix_transpose(matrix, matrixT); 423 | **====================================================================================================*/ 424 | /*====================================================================================================*/ 425 | void Matrix_transpose( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixT ) 426 | { 427 | if(pMatrix != pMatrixT) { 428 | pMatrix->rows = pMatrixT->cols; 429 | pMatrix->cols = pMatrixT->rows; 430 | for(uint16_t i = 0; i < pMatrixT->rows; i++) 431 | for(uint16_t j = 0; j < pMatrixT->cols; j++) 432 | pMatrix->arr[j * pMatrix->cols + i] = pMatrixT->arr[i * pMatrixT->cols + j]; 433 | } 434 | else { 435 | Matrix_TypeDef *matrix = Matrix_copyMatrix(pMatrixT); 436 | pMatrix->rows = matrix->cols; 437 | pMatrix->cols = matrix->rows; 438 | for(uint16_t i = 0; i < matrix->rows; i++) 439 | for(uint16_t j = 0; j < matrix->cols; j++) 440 | pMatrix->arr[j * pMatrix->cols + i] = matrix->arr[i * matrix->cols + j]; 441 | Matrix_delete(matrix); 442 | } 443 | } 444 | /*====================================================================================================*/ 445 | /*====================================================================================================* 446 | **函數 : Matrix_inv 447 | **功能 : Inverse Matrix ( Gaussian Elimination ) 448 | **輸入 : *pMatrix, *pMatrixInv 449 | **輸出 : none 450 | **使用 : matrix = Matrix_inv(matrix, matrixInv); 451 | **====================================================================================================*/ 452 | /*====================================================================================================*/ 453 | void Matrix_inv( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixInv ) 454 | { 455 | if((pMatrix->rows != pMatrix->cols) || (pMatrix->rows != pMatrixInv->rows)) 456 | MATRIX_ERROR(); 457 | 458 | /* check nonsingular */ 459 | // if matrix is nonsingular 460 | // return ERROR; 461 | 462 | Matrix_TypeDef *matrix = Matrix_create(pMatrixInv->rows, pMatrixInv->rows << 1); 463 | Matrix_TypeDef *matrixDiag = Matrix_createDiag(pMatrixInv->rows, 1.0); 464 | Matrix_setMatrix(matrix, pMatrixInv, 0, 0); 465 | Matrix_setMatrix(matrix, matrixDiag, 0, pMatrixInv->cols); 466 | Matrix_delete(matrixDiag); 467 | 468 | Matrix_gaussianElimination(matrix, matrix); 469 | 470 | Matrix_getMatrix(pMatrix, matrix, 0, pMatrixInv->cols); 471 | Matrix_delete(matrix); 472 | } 473 | /*====================================================================================================*/ 474 | /*====================================================================================================* 475 | **函數 : Matrix_gaussianElimination 476 | **功能 : Gaussian Elimination 477 | **輸入 : *pMatrix, *pMatrixGE 478 | **輸出 : none 479 | **使用 : Matrix_gaussianElimination(matrix, matrixGE); 480 | **====================================================================================================*/ 481 | /*====================================================================================================*/ 482 | void Matrix_gaussianElimination( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixGE ) 483 | { 484 | double tmp; 485 | 486 | if(pMatrix != pMatrixGE) 487 | Matrix_copy(pMatrix, pMatrixGE); 488 | 489 | /* left-down to zero */ 490 | for(uint16_t i = 0; i < pMatrix->rows - 1; i++) { 491 | for(uint16_t j = i + 1; j < pMatrix->rows; j++) { 492 | tmp = pMatrix->arr[j * pMatrix->cols + i] / pMatrix->arr[i * pMatrix->cols + i]; 493 | pMatrix->arr[j * pMatrix->cols + i] = 0; 494 | for(uint16_t k = i + 1; k < pMatrix->cols; k++) 495 | pMatrix->arr[j * pMatrix->cols + k] -= tmp * pMatrix->arr[i * pMatrix->cols + k]; 496 | } 497 | } 498 | 499 | /* diagonal to one */ 500 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 501 | tmp = pMatrix->arr[i * pMatrix->cols + i]; 502 | pMatrix->arr[i * pMatrix->cols + i] = 1.0; 503 | for(uint16_t j = i + 1; j < pMatrix->cols; j++) 504 | pMatrix->arr[i * pMatrix->cols + j] = pMatrix->arr[i * pMatrix->cols + j] / tmp; 505 | } 506 | 507 | /* right-up to zero */ 508 | for(int16_t i = pMatrix->rows - 1; i > 0; i--) { 509 | for(int16_t j = i - 1; j >= 0; j--) { 510 | tmp = pMatrix->arr[j * pMatrix->cols + i]; 511 | pMatrix->arr[j * pMatrix->cols + i] = 0; 512 | for(uint16_t k = pMatrix->rows; k < pMatrix->cols; k++) 513 | pMatrix->arr[j * pMatrix->cols + k] -= tmp * pMatrix->arr[i * pMatrix->cols + k]; 514 | } 515 | } 516 | 517 | } 518 | /*====================================================================================================*/ 519 | /*====================================================================================================* 520 | **函數 : Matrix_det 521 | **功能 : Determinant 522 | **輸入 : *pMatrix 523 | **輸出 : data 524 | **使用 : data = Matrix_det(matrix); 525 | **====================================================================================================*/ 526 | /*====================================================================================================*/ 527 | double Matrix_det( Matrix_TypeDef *pMatrix ) 528 | { 529 | double det = 0; 530 | /* 531 | if(matrix->cols != matrix->rows) 532 | return 0; 533 | 534 | */ 535 | return det; 536 | } 537 | /*====================================================================================================*/ 538 | /*====================================================================================================* 539 | **函數 : Matrix_cholesky 540 | **功能 : cholesky 541 | **輸入 : *pMatrix, pMatrixC 542 | **輸出 : none 543 | **使用 : Matrix_cholesky(matrix, matrixC); 544 | **====================================================================================================*/ 545 | /*====================================================================================================*/ 546 | void Matrix_cholesky( Matrix_TypeDef *pMatrix, Matrix_TypeDef *pMatrixC ) 547 | { 548 | /* 549 | if(matrixA->cols != matrixA->rows) 550 | return; 551 | 552 | double tempSum = 0; 553 | for(uint16_t j = 0; j < matrix->cols; j++) { 554 | for(uint16_t i = j; i < matrix->rows; i++) { 555 | tempSum = 0; 556 | if(i == j) { 557 | for(uint16_t k = 0; k < i - 1; k++) 558 | tempSum = tempSum + sqrt_matrix(i, k)^2; 559 | matrix->arr[i * matrix->cols + j] = sqrt(pdMatrix[i * matrix->cols + j] - tempSum); 560 | } 561 | else { 562 | for(uint16_t k = 0; k < j - 1; k++) 563 | tempSum = tempSum + sqrt_matrix(i, k) * sqrt_matrix(j, k); 564 | sqrt_matrix(i, j) = (pdMatrix(i, j) - tempSum) / sqrt_matrix(j, j); 565 | } 566 | } 567 | } 568 | */ 569 | } 570 | /*====================================================================================================*/ 571 | /*====================================================================================================* 572 | **函數 : Matrix_print 573 | **功能 : Print Matrix 574 | **輸入 : *pMatrix 575 | **輸出 : none 576 | **使用 : Matrix_print(matrix); 577 | **====================================================================================================*/ 578 | /*====================================================================================================*/ 579 | void Matrix_print( Matrix_TypeDef *pMatrix ) 580 | { 581 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 582 | printf(" "); 583 | for(uint16_t j = 0; j < pMatrix->cols; j++) { 584 | if(pMatrix->arr[i * pMatrix->cols + j] < 0) 585 | printf("%.5f\t", pMatrix->arr[i * pMatrix->cols + j]); 586 | else 587 | printf("+%.5f\t", pMatrix->arr[i * pMatrix->cols + j]); 588 | } 589 | printf("\n"); 590 | } 591 | } 592 | /*====================================================================================================*/ 593 | /*====================================================================================================*/ 594 | -------------------------------------------------------------------------------- /matrix/matrix.c: -------------------------------------------------------------------------------- 1 | /*====================================================================================================*/ 2 | /*====================================================================================================*/ 3 | #include 4 | #include 5 | #include 6 | 7 | #include "matrix.h" 8 | /*====================================================================================================*/ 9 | /*====================================================================================================*/ 10 | static void MATRIX_ERROR( matrix_t *pMatrix ) 11 | { 12 | printf("\n MATRIX ERROR...\n"); 13 | Matrix_PrintInfo(pMatrix); 14 | while(1); 15 | } 16 | /*====================================================================================================*/ 17 | /*====================================================================================================* 18 | **函數 : Matrix_Clear 19 | **功能 : Clear Matrix 20 | **輸入 : *pMatrix 21 | **輸出 : none 22 | **使用 : Matrix_Clear(matrix); 23 | **====================================================================================================*/ 24 | /*====================================================================================================*/ 25 | void Matrix_Clear( matrix_t *pMatrix ) 26 | { 27 | memset(pMatrix->arr, 0, sizeof(matrix_float_t) * pMatrix->rows * pMatrix->cols); 28 | } 29 | /*====================================================================================================*/ 30 | /*====================================================================================================* 31 | **函數 : Matrix_Init 32 | **功能 : Init Matrix 33 | **輸入 : *pMatrix, *pArray, rows, cols 34 | **輸出 : none 35 | **使用 : Matrix_Init(matrix, array, rows, cols); 36 | **====================================================================================================*/ 37 | /*====================================================================================================*/ 38 | void Matrix_Init( matrix_t *pMatrix, matrix_float_t *pArray, uint16_t rows, uint16_t cols ) 39 | { 40 | pMatrix->mType = MTYPE_NORMAL; 41 | pMatrix->rows = rows; 42 | pMatrix->cols = cols; 43 | pMatrix->arr = pArray; 44 | } 45 | /*====================================================================================================*/ 46 | /*====================================================================================================* 47 | **函數 : Matrix_Create 48 | **功能 : Create Matrix 49 | **輸入 : rows, cols 50 | **輸出 : *matrix 51 | **使用 : matrix = Matrix_Create(rows, cols); 52 | **====================================================================================================*/ 53 | /*====================================================================================================*/ 54 | matrix_t *Matrix_Create( uint16_t rows, uint16_t cols ) 55 | { 56 | matrix_t *matrix = NULL; 57 | 58 | matrix = (matrix_t *)malloc(sizeof(matrix_t)); 59 | matrix->mType = MTYPE_MALLOC; 60 | matrix->rows = rows; 61 | matrix->cols = cols; 62 | matrix->arr = (matrix_float_t *)malloc(sizeof(matrix_float_t) * matrix->rows * matrix->cols); 63 | 64 | return matrix; 65 | } 66 | /*====================================================================================================*/ 67 | /*====================================================================================================* 68 | **函數 : Matrix_CreatePtr 69 | **功能 : Create Matrix Point to Array 70 | **輸入 : *pArray, rows, cols 71 | **輸出 : *matrix 72 | **使用 : matrix = Matrix_CreatePtr(array, rows, cols); 73 | **====================================================================================================*/ 74 | /*====================================================================================================*/ 75 | matrix_t *Matrix_CreatePtr( matrix_float_t *pArray, uint16_t rows, uint16_t cols ) 76 | { 77 | matrix_t *matrix = NULL; 78 | 79 | matrix = (matrix_t *)malloc(sizeof(matrix_t)); 80 | matrix->mType = MTYPE_POINTER; 81 | matrix->rows = rows; 82 | matrix->cols = cols; 83 | matrix->arr = pArray; 84 | 85 | return matrix; 86 | } 87 | /*====================================================================================================*/ 88 | /*====================================================================================================* 89 | **函數 : Matrix_CreateDiag 90 | **功能 : Create Diagonal Matrix 91 | **輸入 : cols, data 92 | **輸出 : *matrix 93 | **使用 : matrix = Matrix_CreateDiag(cols, data); 94 | **====================================================================================================*/ 95 | /*====================================================================================================*/ 96 | matrix_t *Matrix_CreateDiag( uint16_t cols, matrix_float_t data ) 97 | { 98 | matrix_t *matrix = Matrix_Create(cols, cols); 99 | 100 | Matrix_Clear(matrix); 101 | for(uint16_t i = 0; i < cols; i++) 102 | matrix->arr[i * matrix->cols + i] = data; 103 | 104 | return matrix; 105 | } 106 | /*====================================================================================================*/ 107 | /*====================================================================================================* 108 | **函數 : Matrix_Delete 109 | **功能 : Delete Matrix 110 | **輸入 : *matrix 111 | **輸出 : none 112 | **使用 : Matrix_Delete(matrix); 113 | **====================================================================================================*/ 114 | /*====================================================================================================*/ 115 | void Matrix_Delete( matrix_t *matrix ) 116 | { 117 | if(matrix->mType == MTYPE_MALLOC) { 118 | free(matrix->arr); 119 | } 120 | free(matrix); 121 | } 122 | /*====================================================================================================*/ 123 | /*====================================================================================================* 124 | **函數 : Matrix_Copy 125 | **功能 : Copy Matrix 126 | **輸入 : *pMatrixC1, *pMatrixC2 127 | **輸出 : none 128 | **使用 : Matrix_Copy(matrixC1, matrixC2); // matrixC1 = matrixC2 129 | **====================================================================================================*/ 130 | /*====================================================================================================*/ 131 | void Matrix_Copy( matrix_t *pMatrixC1, matrix_t *pMatrixC2 ) 132 | { 133 | if((pMatrixC1->rows != pMatrixC2->rows) || (pMatrixC1->cols != pMatrixC2->cols)) 134 | MATRIX_ERROR(pMatrixC1); 135 | 136 | for(uint32_t i = 0; i < pMatrixC1->rows * pMatrixC1->cols; i++) 137 | pMatrixC1->arr[i] = pMatrixC2->arr[i]; 138 | } 139 | /*====================================================================================================*/ 140 | /*====================================================================================================* 141 | **函數 : Matrix_CopyMatrix 142 | **功能 : Copy & Create Matrix 143 | **輸入 : *pMatrix 144 | **輸出 : *matrix 145 | **使用 : matrix = Matrix_CopyMatrix(matrixCpy); 146 | **====================================================================================================*/ 147 | /*====================================================================================================*/ 148 | matrix_t *Matrix_CopyMatrix( matrix_t *pMatrix ) 149 | { 150 | matrix_t *matrix = Matrix_Create(pMatrix->rows, pMatrix->cols); 151 | 152 | for(uint32_t i = 0; i < matrix->rows * matrix->cols; i++) 153 | matrix->arr[i] = pMatrix->arr[i]; 154 | 155 | return matrix; 156 | } 157 | /*====================================================================================================*/ 158 | /*====================================================================================================* 159 | **函數 : Matrix_CopyArray 160 | **功能 : Copy Array to Matrix 161 | **輸入 : *pArray, rows, cols 162 | **輸出 : *matrix 163 | **使用 : matrix = Matrix_CopyArray(array, row, col); 164 | **====================================================================================================*/ 165 | /*====================================================================================================*/ 166 | matrix_t *Matrix_CopyArray( matrix_float_t *pArray, uint16_t rows, uint16_t cols ) 167 | { 168 | matrix_t *matrix = Matrix_Create(rows, cols); 169 | 170 | for(uint32_t i = 0; i < matrix->rows * matrix->cols; i++) 171 | matrix->arr[i] = pArray[i]; 172 | 173 | return matrix; 174 | } 175 | /*====================================================================================================*/ 176 | /*====================================================================================================* 177 | **函數 : Matrix_Resize 178 | **功能 : Resize Matrix 179 | **輸入 : *pMatrix, rows, cols 180 | **輸出 : none 181 | **使用 : Matrix_Resize(matrix, row, col); 182 | **====================================================================================================*/ 183 | /*====================================================================================================*/ 184 | void Matrix_Resize( matrix_t *pMatrix, uint16_t rows, uint16_t cols ) 185 | { 186 | /* 187 | Matrix_TypeDef *matrix = Matrix_Create(rows, cols); 188 | 189 | Matrix_Clear(matrix); 190 | for(uint32_t i = 0; i < matrix->total; i++) 191 | matrix->arr[i] = array[i]; 192 | 193 | return matrix; 194 | */ 195 | } 196 | /*====================================================================================================*/ 197 | /*====================================================================================================* 198 | **函數 : Matrix_SetData 199 | **功能 : Set Data 200 | **輸入 : *pMatrix, rows, cols, data 201 | **輸出 : None 202 | **使用 : Matrix_SetData(matrix, rows, cols, data); 203 | **====================================================================================================*/ 204 | /*====================================================================================================*/ 205 | void Matrix_SetData( matrix_t *pMatrix, uint16_t rows, uint16_t cols, matrix_float_t data ) 206 | { 207 | if((rows >= pMatrix->rows) && (cols >= pMatrix->cols)) 208 | MATRIX_ERROR(pMatrix); 209 | 210 | pMatrix->arr[rows * pMatrix->cols + cols] = data; 211 | } 212 | /*====================================================================================================*/ 213 | /*====================================================================================================* 214 | **函數 : Matrix_GetData 215 | **功能 : Get Data 216 | **輸入 : *pMatrix, rows, cols 217 | **輸出 : data 218 | **使用 : data = Matrix_GetData(matrix, rows, cols); 219 | **====================================================================================================*/ 220 | /*====================================================================================================*/ 221 | matrix_float_t Matrix_GetData( matrix_t *pMatrix, uint16_t rows, uint16_t cols ) 222 | { 223 | if((rows >= pMatrix->rows) && (cols >= pMatrix->cols)) 224 | MATRIX_ERROR(pMatrix); 225 | 226 | return pMatrix->arr[rows * pMatrix->cols + cols]; 227 | } 228 | /*====================================================================================================*/ 229 | /*====================================================================================================* 230 | **函數 : Matrix_SetDiag 231 | **功能 : Set Diagonal Matrix 232 | **輸入 : *pMatrix, data 233 | **輸出 : none 234 | **使用 : Matrix_SetDiag(matrix, data); 235 | **====================================================================================================*/ 236 | /*====================================================================================================*/ 237 | void Matrix_SetDiag( matrix_t *pMatrix, matrix_float_t data ) 238 | { 239 | if(pMatrix->rows != pMatrix->cols) 240 | MATRIX_ERROR(pMatrix); 241 | 242 | for(uint16_t i = 0; i < pMatrix->cols; i++) 243 | pMatrix->arr[i * pMatrix->cols + i] = data; 244 | } 245 | /*====================================================================================================*/ 246 | /*====================================================================================================* 247 | **函數 : Matrix_GetDiag 248 | **功能 : Get Diagonal 249 | **輸入 : *pMatrix, pMatrixD 250 | **輸出 : none 251 | **使用 : Matrix_GetDiag(matrix, matrixD); 252 | **====================================================================================================*/ 253 | /*====================================================================================================*/ 254 | void Matrix_GetDiag( matrix_t *pMatrix, matrix_t *pMatrixD ) 255 | { 256 | if((pMatrix->rows != pMatrix->cols) || (pMatrixD->rows != pMatrixD->cols) || (pMatrix->rows != pMatrixD->cols)) 257 | MATRIX_ERROR(pMatrix); 258 | 259 | if(pMatrix != pMatrixD) { 260 | Matrix_Clear(pMatrix); 261 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 262 | pMatrix->arr[i * pMatrix->cols + i] = pMatrixD->arr[i * pMatrixD->rows + i]; 263 | } 264 | } 265 | else { 266 | matrix_t *matrix = Matrix_CopyMatrix(pMatrixD); 267 | for(uint16_t i = 0; i < pMatrix->rows; i++) 268 | pMatrix->arr[i * pMatrix->cols + i] = matrix->arr[i * matrix->rows + i]; 269 | } 270 | } 271 | /*====================================================================================================*/ 272 | /*====================================================================================================* 273 | **函數 : Matrix_SetMatrix 274 | **功能 : Set Matrix 275 | **輸入 : *pMatrix, *pMatrixS, rows_pos, cols_pos 276 | **輸出 : none 277 | **使用 : Matrix_SetMatrix(matrix, matrixS, rows_pos, cols_pos); 278 | **====================================================================================================*/ 279 | /*====================================================================================================*/ 280 | void Matrix_SetMatrix( matrix_t *pMatrix, matrix_t *pMatrixS, uint16_t rows_pos, uint16_t cols_pos ) 281 | { 282 | if(((rows_pos + pMatrixS->rows) > pMatrix->rows) || ((cols_pos + pMatrixS->cols) > pMatrix->cols)) 283 | MATRIX_ERROR(pMatrix); 284 | 285 | uint32_t cnt = 0; 286 | for(uint16_t i = rows_pos; i < rows_pos + pMatrixS->rows; i++) { 287 | for(uint16_t j = cols_pos; j < cols_pos + pMatrixS->cols; j++) { 288 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixS->arr[cnt++]; 289 | } 290 | } 291 | } 292 | /*====================================================================================================*/ 293 | /*====================================================================================================* 294 | **函數 : Matrix_GetMatrix 295 | **功能 : Get Matrix 296 | **輸入 : *pMatrixG, *pMatrix, rows_pos, cols_pos 297 | **輸出 : none 298 | **使用 : Matrix_GetMatrix(matrixG, matrix, rows_pos, cols_pos); 299 | **====================================================================================================*/ 300 | /*====================================================================================================*/ 301 | void Matrix_GetMatrix( matrix_t *pMatrixG, matrix_t *pMatrix, uint16_t rows_pos, uint16_t cols_pos ) 302 | { 303 | if(((rows_pos + pMatrixG->rows) > pMatrix->rows) || ((cols_pos + pMatrixG->cols) > pMatrix->cols)) 304 | MATRIX_ERROR(pMatrix); 305 | 306 | uint32_t cnt = 0; 307 | for(uint16_t i = rows_pos; i < rows_pos + pMatrixG->rows; i++) { 308 | for(uint16_t j = cols_pos; j < cols_pos + pMatrixG->cols; j++) { 309 | pMatrixG->arr[cnt++] = pMatrix->arr[i * pMatrix->cols + j]; 310 | } 311 | } 312 | } 313 | /*====================================================================================================*/ 314 | /*====================================================================================================* 315 | **函數 : Matrix_Add 316 | **功能 : Matrix Addition 317 | **輸入 : *pMatrix, *pMatrixA1, pMatrixA2 318 | **輸出 : none 319 | **使用 : Matrix_Add(matrix, matrixA1, matrixA2); 320 | **====================================================================================================*/ 321 | /*====================================================================================================*/ 322 | void Matrix_Add( matrix_t *pMatrix, matrix_t *pMatrixA1, matrix_t *pMatrixA2 ) 323 | { 324 | if((pMatrix->rows != pMatrixA1->rows) || (pMatrix->cols != pMatrixA1->cols) || 325 | (pMatrix->rows != pMatrixA2->rows) || (pMatrix->cols != pMatrixA2->cols)) 326 | MATRIX_ERROR(pMatrix); 327 | 328 | for(uint16_t i = 0; i < pMatrix->rows; i++) 329 | for(uint16_t j = 0; j < pMatrix->cols; j++) 330 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixA1->arr[i * pMatrixA1->cols + j] + pMatrixA2->arr[i * pMatrixA2->cols + j]; 331 | } 332 | /*====================================================================================================*/ 333 | /*====================================================================================================* 334 | **函數 : Matrix_Sub 335 | **功能 : Matrix Subtraction 336 | **輸入 : *pMatrix, *pMatrixS1, *pMatrixS2 337 | **輸出 : none 338 | **使用 : Matrix_Sub(matrix, matrixS1, matrixS2); 339 | **====================================================================================================*/ 340 | /*====================================================================================================*/ 341 | void Matrix_Sub( matrix_t *pMatrix, matrix_t *pMatrixS1, matrix_t *pMatrixS2 ) 342 | { 343 | if((pMatrix->rows != pMatrixS1->rows) || (pMatrix->cols != pMatrixS1->cols) || 344 | (pMatrix->rows != pMatrixS2->rows) || (pMatrix->cols != pMatrixS2->cols)) 345 | MATRIX_ERROR(pMatrix); 346 | 347 | for(uint16_t i = 0; i < pMatrix->rows; i++) 348 | for(uint16_t j = 0; j < pMatrix->cols; j++) 349 | pMatrix->arr[i * pMatrix->cols + j] = pMatrixS1->arr[i * pMatrixS1->cols + j] - pMatrixS2->arr[i * pMatrixS2->cols + j]; 350 | } 351 | /*====================================================================================================*/ 352 | /*====================================================================================================* 353 | **函數 : Matrix_Mul 354 | **功能 : Matrix Multiplication 355 | **輸入 : *pMatrix, *pMatrixM1, *pMatrixM2 356 | **輸出 : none 357 | **使用 : Matrix_Mul(matrix, matrixM1, matrixM2); 358 | **====================================================================================================*/ 359 | /*====================================================================================================*/ 360 | void Matrix_Mul( matrix_t *pMatrix, matrix_t *pMatrixM1, matrix_t *pMatrixM2 ) 361 | { 362 | if((pMatrixM1->cols != pMatrixM2->rows) || (pMatrix->rows != pMatrixM1->rows) || (pMatrix->cols != pMatrixM2->cols)) 363 | MATRIX_ERROR(pMatrix); 364 | 365 | if(pMatrix == pMatrixM1) { 366 | matrix_t *matrix = Matrix_CopyMatrix(pMatrixM1); 367 | Matrix_Clear(pMatrix); 368 | for(uint16_t i = 0; i < matrix->rows; i++) { 369 | for(uint16_t j = 0; j < pMatrixM2->cols; j++) { 370 | for(uint16_t k = 0; k < matrix->cols; k++) { 371 | pMatrix->arr[i * pMatrix->cols + j] += matrix->arr[i * matrix->cols + k] * pMatrixM2->arr[k * pMatrixM2->cols + j]; 372 | } 373 | } 374 | } 375 | } 376 | /* 377 | else if(pMatrix == pMatrixM2) { 378 | matrix_t *matrix = Matrix_CopyMatrix(pMatrixM2); 379 | Matrix_Clear(pMatrix); 380 | for(uint16_t i = 0; i < pMatrixM1->rows; i++) { 381 | for(uint16_t j = 0; j < matrix->cols; j++) { 382 | for(uint16_t k = 0; k < pMatrixM1->cols; k++) { 383 | pMatrix->arr[i * pMatrix->cols + j] += pMatrixM1->arr[i * pMatrixM1->cols + k] * matrix->arr[k * matrix->cols + j]; 384 | } 385 | } 386 | } 387 | } 388 | */ 389 | else { 390 | Matrix_Clear(pMatrix); 391 | for(uint16_t i = 0; i < pMatrixM1->rows; i++) { 392 | for(uint16_t j = 0; j < pMatrixM2->cols; j++) { 393 | for(uint16_t k = 0; k < pMatrixM1->cols; k++) { 394 | pMatrix->arr[i * pMatrix->cols + j] += pMatrixM1->arr[i * pMatrixM1->cols + k] * pMatrixM2->arr[k * pMatrixM2->cols + j]; 395 | } 396 | } 397 | } 398 | } 399 | } 400 | /*====================================================================================================*/ 401 | /*====================================================================================================* 402 | **函數 : Matrix_MulNumb 403 | **功能 : Matrix Multiplication 404 | **輸入 : *pMatrix, *pMatrixM1, number 405 | **輸出 : none 406 | **使用 : Matrix_MulNumb(matrix, matrixM1, number); 407 | **====================================================================================================*/ 408 | /*====================================================================================================*/ 409 | void Matrix_MulNumb( matrix_t *pMatrix, matrix_t *pMatrixM1, matrix_float_t number ) 410 | { 411 | if((pMatrix->cols != pMatrixM1->cols) || (pMatrix->rows != pMatrixM1->rows)) 412 | MATRIX_ERROR(pMatrix); 413 | 414 | for(uint32_t i = 0; i < pMatrix->rows * pMatrix->cols; i++) { 415 | pMatrix->arr[i] = pMatrixM1->arr[i] * number; 416 | } 417 | } 418 | /*====================================================================================================*/ 419 | /*====================================================================================================* 420 | **函數 : Matrix_Transpose 421 | **功能 : Matrix Transpose 422 | **輸入 : *pMatrix, *pMatrixT 423 | **輸出 : none 424 | **使用 : Matrix_Transpose(matrix, matrixT); 425 | **====================================================================================================*/ 426 | /*====================================================================================================*/ 427 | void Matrix_Transpose( matrix_t *pMatrix, matrix_t *pMatrixT ) 428 | { 429 | if(pMatrix != pMatrixT) { 430 | pMatrix->rows = pMatrixT->cols; 431 | pMatrix->cols = pMatrixT->rows; 432 | for(uint16_t i = 0; i < pMatrixT->rows; i++) 433 | for(uint16_t j = 0; j < pMatrixT->cols; j++) 434 | pMatrix->arr[j * pMatrix->cols + i] = pMatrixT->arr[i * pMatrixT->cols + j]; 435 | } 436 | else { 437 | matrix_t *matrix = Matrix_CopyMatrix(pMatrixT); 438 | pMatrix->rows = matrix->cols; 439 | pMatrix->cols = matrix->rows; 440 | for(uint16_t i = 0; i < matrix->rows; i++) 441 | for(uint16_t j = 0; j < matrix->cols; j++) 442 | pMatrix->arr[j * pMatrix->cols + i] = matrix->arr[i * matrix->cols + j]; 443 | Matrix_Delete(matrix); 444 | } 445 | } 446 | /*====================================================================================================*/ 447 | /*====================================================================================================* 448 | **函數 : Matrix_Inv 449 | **功能 : Inverse Matrix ( Gaussian Elimination ) 450 | **輸入 : *pMatrix, *pMatrixInv 451 | **輸出 : none 452 | **使用 : Matrix_Inv(matrix, matrixInv); 453 | **====================================================================================================*/ 454 | /*====================================================================================================*/ 455 | void Matrix_Inv( matrix_t *pMatrix, matrix_t *pMatrixInv ) 456 | { 457 | if((pMatrix->rows != pMatrix->cols) || (pMatrix->rows != pMatrixInv->rows)) 458 | MATRIX_ERROR(pMatrix); 459 | 460 | /* check nonsingular */ 461 | // if matrix is nonsingular 462 | // return ERROR; 463 | 464 | matrix_t *matrix = Matrix_Create(pMatrixInv->rows, pMatrixInv->rows << 1); 465 | matrix_t *matrixDiag = Matrix_CreateDiag(pMatrixInv->rows, 1.0); 466 | Matrix_SetMatrix(matrix, pMatrixInv, 0, 0); 467 | Matrix_SetMatrix(matrix, matrixDiag, 0, pMatrixInv->cols); 468 | Matrix_Delete(matrixDiag); 469 | 470 | Matrix_GaussianElimination(matrix, matrix); 471 | 472 | Matrix_GetMatrix(pMatrix, matrix, 0, pMatrixInv->cols); 473 | Matrix_Delete(matrix); 474 | } 475 | /*====================================================================================================*/ 476 | /*====================================================================================================* 477 | **函數 : Matrix_GaussianElimination 478 | **功能 : Gaussian Elimination 479 | **輸入 : *pMatrix, *pMatrixGE 480 | **輸出 : none 481 | **使用 : Matrix_GaussianElimination(matrix, matrixGE); 482 | **====================================================================================================*/ 483 | /*====================================================================================================*/ 484 | void Matrix_GaussianElimination( matrix_t *pMatrix, matrix_t *pMatrixGE ) 485 | { 486 | matrix_float_t tmp; 487 | 488 | if(pMatrix != pMatrixGE) 489 | Matrix_Copy(pMatrix, pMatrixGE); 490 | 491 | /* left-down to zero */ 492 | for(uint16_t i = 0; i < pMatrix->rows - 1; i++) { 493 | for(uint16_t j = i + 1; j < pMatrix->rows; j++) { 494 | tmp = pMatrix->arr[j * pMatrix->cols + i] / pMatrix->arr[i * pMatrix->cols + i]; 495 | pMatrix->arr[j * pMatrix->cols + i] = 0; 496 | for(uint16_t k = i + 1; k < pMatrix->cols; k++) 497 | pMatrix->arr[j * pMatrix->cols + k] -= tmp * pMatrix->arr[i * pMatrix->cols + k]; 498 | } 499 | } 500 | 501 | /* diagonal to one */ 502 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 503 | tmp = pMatrix->arr[i * pMatrix->cols + i]; 504 | pMatrix->arr[i * pMatrix->cols + i] = 1.0; 505 | for(uint16_t j = i + 1; j < pMatrix->cols; j++) 506 | pMatrix->arr[i * pMatrix->cols + j] = pMatrix->arr[i * pMatrix->cols + j] / tmp; 507 | } 508 | 509 | /* right-up to zero */ 510 | for(int16_t i = pMatrix->rows - 1; i > 0; i--) { 511 | for(int16_t j = i - 1; j >= 0; j--) { 512 | tmp = pMatrix->arr[j * pMatrix->cols + i]; 513 | pMatrix->arr[j * pMatrix->cols + i] = 0; 514 | for(uint16_t k = pMatrix->rows; k < pMatrix->cols; k++) 515 | pMatrix->arr[j * pMatrix->cols + k] -= tmp * pMatrix->arr[i * pMatrix->cols + k]; 516 | } 517 | } 518 | 519 | } 520 | /*====================================================================================================*/ 521 | /*====================================================================================================* 522 | **函數 : Matrix_Det 523 | **功能 : Determinant 524 | **輸入 : *pMatrix 525 | **輸出 : data 526 | **使用 : data = Matrix_Det(matrix); 527 | **====================================================================================================*/ 528 | /*====================================================================================================*/ 529 | matrix_float_t Matrix_Det( matrix_t *pMatrix ) 530 | { 531 | matrix_float_t det = 0; 532 | /* 533 | if(matrix->cols != matrix->rows) 534 | return 0; 535 | 536 | */ 537 | return det; 538 | } 539 | /*====================================================================================================*/ 540 | /*====================================================================================================* 541 | **函數 : Matrix_Cholesky 542 | **功能 : cholesky 543 | **輸入 : *pMatrix, pMatrixC 544 | **輸出 : none 545 | **使用 : Matrix_Cholesky(matrix, matrixC); 546 | **====================================================================================================*/ 547 | /*====================================================================================================*/ 548 | void Matrix_Cholesky( matrix_t *pMatrix, matrix_t *pMatrixC ) 549 | { 550 | /* 551 | if(matrixA->cols != matrixA->rows) 552 | return; 553 | 554 | matrix_arr_t tempSum = 0; 555 | for(uint16_t j = 0; j < matrix->cols; j++) { 556 | for(uint16_t i = j; i < matrix->rows; i++) { 557 | tempSum = 0; 558 | if(i == j) { 559 | for(uint16_t k = 0; k < i - 1; k++) 560 | tempSum = tempSum + sqrt_matrix(i, k)^2; 561 | matrix->arr[i * matrix->cols + j] = sqrt(pdMatrix[i * matrix->cols + j] - tempSum); 562 | } 563 | else { 564 | for(uint16_t k = 0; k < j - 1; k++) 565 | tempSum = tempSum + sqrt_matrix(i, k) * sqrt_matrix(j, k); 566 | sqrt_matrix(i, j) = (pdMatrix(i, j) - tempSum) / sqrt_matrix(j, j); 567 | } 568 | } 569 | } 570 | */ 571 | } 572 | /*====================================================================================================*/ 573 | /*====================================================================================================* 574 | **函數 : Matrix_Print 575 | **功能 : Print Matrix 576 | **輸入 : *pMatrix 577 | **輸出 : none 578 | **使用 : Matrix_Print(matrix); 579 | **====================================================================================================*/ 580 | /*====================================================================================================*/ 581 | void Matrix_Print( matrix_t *pMatrix ) 582 | { 583 | for(uint16_t i = 0; i < pMatrix->rows; i++) { 584 | printf(" "); 585 | for(uint16_t j = 0; j < pMatrix->cols; j++) { 586 | printf("%-+14f ", pMatrix->arr[i * pMatrix->cols + j]); 587 | } 588 | printf("\n"); 589 | } 590 | } 591 | /*====================================================================================================*/ 592 | /*====================================================================================================* 593 | **函數 : Matrix_PrintInfo 594 | **功能 : Print Matrix Information 595 | **輸入 : *pMatrix 596 | **輸出 : none 597 | **使用 : Matrix_PrintInfo(matrix); 598 | **====================================================================================================*/ 599 | /*====================================================================================================*/ 600 | void Matrix_PrintInfo( matrix_t *pMatrix ) 601 | { 602 | printf("matrix.rows = %d\n", pMatrix->rows); 603 | printf("matrix.cols = %d\n", pMatrix->cols); 604 | printf("matrix.arr = %d\n", (uint32_t)pMatrix->arr); 605 | if(pMatrix->mType == MTYPE_NORMAL) 606 | printf("matrix.mType = NORMAL\n"); 607 | else if(pMatrix->mType == MTYPE_MALLOC) 608 | printf("matrix.mType = MALLOC\n"); 609 | else 610 | printf("matrix.mType = POINTER\n"); 611 | printf("\n"); 612 | } 613 | /*====================================================================================================*/ 614 | /*====================================================================================================*/ 615 | --------------------------------------------------------------------------------