├── OneRCDesignDoc ├── 20629.jpg ├── 20636.jpg ├── FlightGearSim.png ├── OneRCFW_block_diagram.png ├── PCB_block_diagram.png ├── PWM_PPM_generator.png ├── arduino_flyctrl_layout_20171217_v1_8.png ├── 姿態 - 矩陣旋轉.pdf └── 姿態模擬結果.pdf ├── OneRCFW ├── OneRCAirplane │ ├── OneRCAirplane.cpp │ ├── OneRCAirplane.h │ └── OneRCAirplane.ino └── libraries │ └── OneRCLib │ ├── OneRCLib.cpp │ ├── OneRCLib.h │ ├── adc_drv.cpp │ ├── adc_drv.h │ ├── ahrs.cpp │ ├── ahrs.h │ ├── coding_template.cpp │ ├── crc_ccitt.cpp │ ├── crc_ccitt.h │ ├── debug.cpp │ ├── debug.h │ ├── failsafe.cpp │ ├── failsafe.h │ ├── gps.cpp │ ├── gps.h │ ├── i2c_drv.cpp │ ├── i2c_drv.h │ ├── imu_ctrl.cpp │ ├── imu_ctrl.h │ ├── leds_ctrl.cpp │ ├── leds_ctrl.h │ ├── math_lib.cpp │ ├── math_lib.h │ ├── mcu_protocol.cpp │ ├── mcu_protocol.h │ ├── mpu6050_drv.cpp │ ├── mpu6050_drv.h │ ├── pid.cpp │ ├── pid.h │ ├── pin_change.cpp │ ├── pin_change.h │ ├── rc_in.cpp │ ├── rc_in.h │ ├── rc_out.cpp │ ├── rc_out.h │ ├── rom_drv.cpp │ ├── rom_drv.h │ ├── timers_drv.cpp │ ├── timers_drv.h │ ├── uart_drv.cpp │ ├── uart_drv.h │ ├── uart_sim.cpp │ ├── uart_sim.h │ ├── uart_stream.cpp │ ├── uart_stream.h │ ├── ublox6m_drv.cpp │ └── ublox6m_drv.h ├── OneRCFlightGearConf ├── .fgfsrc └── MAVLink.xml ├── OneRCGUI ├── MP_CRC.py ├── MP_GUI.fbp ├── MP_GUI.py ├── MP_decoder.py ├── MP_fdm_nmea.py ├── MP_fdm_sim.py ├── MP_frames.py ├── MP_handler.py ├── MP_main.py ├── MP_plot.py └── install.txt ├── OneRCSchematic └── OneRCSchematic_v1 │ ├── OneRCLibLogo.bmp │ ├── OneRCLibLogo.pptx │ ├── OneRCLibLogo_8bits.bmp │ ├── OneRCLogo.lbr │ ├── OneRCSchematic_v1_8.brd │ ├── OneRCSchematic_v1_8.docx │ ├── OneRCSchematic_v1_8.pdf │ ├── OneRCSchematic_v1_8.png │ ├── OneRCSchematic_v1_8.sch │ ├── OneRCSchematic_v1_8.xlsx │ ├── OneRCSchematic_v1_8_BOM.pdf │ └── OneRCSchematic_v1_8_Gerber │ ├── OneRCSchematic_v1_8.GBL │ ├── OneRCSchematic_v1_8.GBO │ ├── OneRCSchematic_v1_8.GBS │ ├── OneRCSchematic_v1_8.GKO │ ├── OneRCSchematic_v1_8.GML │ ├── OneRCSchematic_v1_8.GTL │ ├── OneRCSchematic_v1_8.GTO │ ├── OneRCSchematic_v1_8.GTP │ ├── OneRCSchematic_v1_8.GTS │ ├── OneRCSchematic_v1_8.TXT │ ├── OneRCSchematic_v1_8.dri │ └── OneRCSchematic_v1_8.gpi └── README.md /OneRCDesignDoc/20629.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/20629.jpg -------------------------------------------------------------------------------- /OneRCDesignDoc/20636.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/20636.jpg -------------------------------------------------------------------------------- /OneRCDesignDoc/FlightGearSim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/FlightGearSim.png -------------------------------------------------------------------------------- /OneRCDesignDoc/OneRCFW_block_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/OneRCFW_block_diagram.png -------------------------------------------------------------------------------- /OneRCDesignDoc/PCB_block_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/PCB_block_diagram.png -------------------------------------------------------------------------------- /OneRCDesignDoc/PWM_PPM_generator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/PWM_PPM_generator.png -------------------------------------------------------------------------------- /OneRCDesignDoc/arduino_flyctrl_layout_20171217_v1_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/arduino_flyctrl_layout_20171217_v1_8.png -------------------------------------------------------------------------------- /OneRCDesignDoc/姿態 - 矩陣旋轉.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/姿態 - 矩陣旋轉.pdf -------------------------------------------------------------------------------- /OneRCDesignDoc/姿態模擬結果.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCDesignDoc/姿態模擬結果.pdf -------------------------------------------------------------------------------- /OneRCFW/OneRCAirplane/OneRCAirplane.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file OneRCAirplane.h 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef ONERCAIRPLANE_H_ 17 | #define ONERCAIRPLANE_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | #define AIRPLANE_CONFIG_ID 0x315f4150 /* "1_AP" */ 27 | 28 | #define AIRPLANE_FW_DATE 0x20180303 29 | 30 | #define AIRPLANE_DBG_PIN_HIGH() do{ \ 31 | PORTC |= _BV(PORTC0); \ 32 | }while(0) 33 | 34 | #define AIRPLANE_DBG_PIN_LOW() do{ \ 35 | PORTC &= ~_BV(PORTC0); \ 36 | }while(0) 37 | 38 | #define AIRPLANE_CFG_ROM_ADDR 0x0 39 | 40 | #define AIRPLANE_CTRL_LOOP_PERIOD 5000 /* 5000 us = 5.0 ms */ 41 | #define AIRPLANE_CTRL_LOOP_DELAY_THR 5500 /* 5500 us = 5.5 ms */ 42 | 43 | #define AIRPLANE_CHK_CFG_MODE_TIMEOUT 1000 /* 1000 ms = 1 second */ 44 | 45 | #define AIRPLANE_RC_CALI_SMOOTH_PERIOD 10 /* 10 ms */ 46 | #define AIRPLANE_RC_CALI_SMOOTH_CNT 300 /* 300 times */ 47 | 48 | #define AIRPLANE_GPS_HOME_SAMPLE_CNT 64 /* Home position sample count */ 49 | 50 | /* Maximum pitch compensation angle limitation +- N */ 51 | #define AIRPLANE_BANK_TURN_MAX_PITCH 5.0 /* Degree */ 52 | 53 | #define AIRPLANE_BANK_TURN_MAX_GAIN 1.5 /* +- 1.5 */ 54 | 55 | /* 56 | * Gain setting for mapping roll [180 .. 0 .. -180] to pitch [N .. 0 .. -N] degree 57 | * Say pitch_3_degree / (1.0 - cos(roll_30_degree)) = 22.3923048447, 58 | * means we will set pitch angle to 3.0 degree when roll angle is equal to +- 30 degree. 59 | */ 60 | #define AIRPLANE_BANK_TURN_PITCH_GAIN 22.3923048447 61 | 62 | /* Airplane status snapshot function */ 63 | #define AIRPLANE_STATUS_SNAPSHOT_EN false 64 | 65 | 66 | /* 67 | ******************************************************************************* 68 | * Data type definition 69 | ******************************************************************************* 70 | */ 71 | 72 | typedef enum airplane_cruise_state{ 73 | AIRPLANE_CRUISE_FORWARDTO_WPT = 0, 74 | AIRPLANE_CRUISE_AWAYFROM_WPT, 75 | }__attribute__((packed)) AIRPLANE_CRUISE_STATE; 76 | 77 | typedef enum airplane_nav_state{ 78 | AIRPLANE_NAV_WAIT_NMEA_FRM = 0, 79 | AIRPLANE_NAV_UPDATE_MOVING_BEARING, 80 | AIRPLANE_NAV_UPDATE_WAYPOINT_BEARING, 81 | AIRPLANE_NAV_UPDATE_RELATIVE_BEARING, 82 | }__attribute__((packed)) AIRPLANE_NAV_STATE; 83 | 84 | typedef enum airplane_type{ 85 | AIRPLANE_NORMAL = 0, 86 | AIRPLANE_DELTA, 87 | AIRPLANE_VTAIL, 88 | }__attribute__((packed)) AIRPLANE_TYPE; 89 | 90 | typedef enum airplane_fly_mode{ 91 | AIRPLANE_MANUAL_FLY = 0, 92 | AIRPLANE_SELF_STABILIZE, 93 | AIRPLANE_RETURN_TO_HOME, 94 | }__attribute__((packed)) AIRPLANE_FLY_MODE; 95 | 96 | 97 | /* 98 | ******************************************************************************* 99 | * Global variables 100 | ******************************************************************************* 101 | */ 102 | 103 | 104 | /* 105 | ******************************************************************************* 106 | * Public functions declaration 107 | ******************************************************************************* 108 | */ 109 | 110 | 111 | /* 112 | ******************************************************************************* 113 | * Private functions declaration 114 | ******************************************************************************* 115 | */ 116 | 117 | 118 | /* 119 | ******************************************************************************* 120 | * Public functions 121 | ******************************************************************************* 122 | */ 123 | 124 | void LED_Blink(uint32_t on_millis, uint32_t off_millis); 125 | 126 | 127 | /* 128 | ******************************************************************************* 129 | * Private functions 130 | ******************************************************************************* 131 | */ 132 | 133 | 134 | #endif // ONERCAIRPLANE_H_ 135 | -------------------------------------------------------------------------------- /OneRCFW/OneRCAirplane/OneRCAirplane.ino: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file OneRCAirplane.ino 11 | * @brief The actual setup and loop function are located in OneRCAirplane.cpp 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | 17 | /* 18 | ******************************************************************************* 19 | * Constant value definition 20 | ******************************************************************************* 21 | */ 22 | 23 | /* 24 | ******************************************************************************* 25 | * Data type definition 26 | ******************************************************************************* 27 | */ 28 | 29 | 30 | /* 31 | ******************************************************************************* 32 | * Global variables 33 | ******************************************************************************* 34 | */ 35 | 36 | 37 | /* 38 | ******************************************************************************* 39 | * Public functions declaration 40 | ******************************************************************************* 41 | */ 42 | 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Private functions declaration 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Public functions 55 | ******************************************************************************* 56 | */ 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Private functions 62 | ******************************************************************************* 63 | */ 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/OneRCLib.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file OneRCLIB.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | 17 | /* 18 | ******************************************************************************* 19 | * Constant value definition 20 | ******************************************************************************* 21 | */ 22 | 23 | 24 | /* 25 | ******************************************************************************* 26 | * Data type definition 27 | ******************************************************************************* 28 | */ 29 | 30 | 31 | /* 32 | ******************************************************************************* 33 | * Global variables 34 | ******************************************************************************* 35 | */ 36 | 37 | 38 | /* 39 | ******************************************************************************* 40 | * Public functions declaration 41 | ******************************************************************************* 42 | */ 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Private functions declaration 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Public functions 55 | ******************************************************************************* 56 | */#include 57 | 58 | /** 59 | * function_example - Function example 60 | * 61 | * @param [in] input Example input. 62 | * @param [out] *p_output Example output. 63 | * 64 | * @return [int] Function executing result. 65 | * @retval [0] Success. 66 | * @retval [-1] Fail. 67 | * 68 | */ 69 | 70 | 71 | /* 72 | ******************************************************************************* 73 | * Private functions 74 | ******************************************************************************* 75 | */ 76 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/OneRCLib.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file OneRCLIB.h 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef ONERCLIB_H_ 17 | #define ONERCLIB_H_ 18 | 19 | #include "crc_ccitt.h" 20 | #include "leds_ctrl.h" 21 | #include "ahrs.h" 22 | #include "i2c_drv.h" 23 | #include "imu_ctrl.h" 24 | #include "rc_in.h" 25 | #include "failsafe.h" 26 | #include "timers_drv.h" 27 | #include "rc_out.h" 28 | #include "uart_drv.h" 29 | #include "uart_stream.h" 30 | #include "pid.h" 31 | #include "mcu_protocol.h" 32 | #include "adc_drv.h" 33 | #include "rom_drv.h" 34 | #include "uart_sim.h" 35 | #include "pin_change.h" 36 | #include "debug.h" 37 | #include "gps.h" 38 | #include "ublox6m_drv.h" 39 | #include "math_lib.h" 40 | 41 | 42 | /* 43 | ******************************************************************************* 44 | * Constant value definition 45 | ******************************************************************************* 46 | */ 47 | 48 | 49 | /* 50 | ******************************************************************************* 51 | * Data type definition 52 | ******************************************************************************* 53 | */ 54 | 55 | 56 | /* 57 | ******************************************************************************* 58 | * Global variables 59 | ******************************************************************************* 60 | */ 61 | 62 | 63 | /* 64 | ******************************************************************************* 65 | * Public functions declaration 66 | ******************************************************************************* 67 | */ 68 | 69 | 70 | /* 71 | ******************************************************************************* 72 | * Private functions declaration 73 | ******************************************************************************* 74 | */ 75 | 76 | 77 | /* 78 | ******************************************************************************* 79 | * Public functions 80 | ******************************************************************************* 81 | */ 82 | 83 | /** 84 | * function_example - Function example 85 | * 86 | * @param [in] input Example input. 87 | * @param [out] *p_output Example output. 88 | * 89 | * @return [int] Function executing result. 90 | * @retval [0] Success. 91 | * @retval [-1] Fail. 92 | * 93 | */ 94 | 95 | 96 | /* 97 | ******************************************************************************* 98 | * Private functions 99 | ******************************************************************************* 100 | */ 101 | 102 | 103 | #endif // ONERCLIB_H_ 104 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/adc_drv.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file adc_drv.cpp 11 | * @brief ADC functions. 12 | * 13 | * Reference: 14 | * Measuring VCC via the bandgap 15 | * https://jeelabs.org/2012/05/04/measuring-vcc-via-the-bandgap/ 16 | * 17 | * Abbreviations: 18 | * ADC - Analog-to-Digital Converter 19 | * 20 | * Pin/Channel mapping: 21 | * ADC CH 0 = Arduino A6 / Analog 22 | * ADC CH 1 = Arduino A1 / AVR PC1 23 | * ADC CH 2 = Arduino A2 / AVR PC2 24 | * ADC CH 3 = Arduino A3 / AVR PC3 25 | * ADC CH 4 = Arduino A7 / Analog 26 | * 27 | * Hardware: 28 | * Analog-to-Digital Converter 29 | * 30 | * Interrupts: 31 | * None 32 | * 33 | * @author Y.S.Kuo in Hsinchu 34 | ******************************************************************************* 35 | */ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "uart_stream.h" 43 | #include "adc_drv.h" 44 | #include "timers_drv.h" 45 | 46 | 47 | /* 48 | ******************************************************************************* 49 | * Constant value definition 50 | ******************************************************************************* 51 | */ 52 | 53 | #define ADC_CH0_ARDU_PIN A6 54 | #define ADC_CH1_ARDU_PIN A1 55 | #define ADC_CH2_ARDU_PIN A2 56 | #define ADC_CH3_ARDU_PIN A3 57 | #define ADC_CH4_ARDU_PIN A7 58 | 59 | #define ADC_ANALOG_VOLTAGE 5.0 /* External analog VCC */ 60 | #define ADC_BANDGAP_VOLTAGE 1.1 /* Internal bandgap voltage */ 61 | 62 | 63 | /* 64 | ******************************************************************************* 65 | * Data type definition 66 | ******************************************************************************* 67 | */ 68 | 69 | 70 | /* 71 | ******************************************************************************* 72 | * Global variables 73 | ******************************************************************************* 74 | */ 75 | 76 | /* 77 | ******************************************************************************* 78 | * Private functions declaration 79 | ******************************************************************************* 80 | */ 81 | 82 | static void ADC_SetMux(uint8_t adc_channel); 83 | static uint16_t ADC_SingleConversion(); 84 | 85 | 86 | /* 87 | ******************************************************************************* 88 | * Public functions implementation 89 | ******************************************************************************* 90 | */ 91 | 92 | /** 93 | * ADC_Init - Function to initialize Atmel ADC pins and registers. 94 | * 95 | * @param [none] 96 | * @return [none] 97 | * 98 | */ 99 | int8_t ADC_Init() 100 | { 101 | /* ADC pins initialization */ 102 | pinMode(ADC_CH0_ARDU_PIN, INPUT); 103 | pinMode(ADC_CH1_ARDU_PIN, INPUT); 104 | pinMode(ADC_CH2_ARDU_PIN, INPUT); 105 | pinMode(ADC_CH3_ARDU_PIN, INPUT); 106 | pinMode(ADC_CH4_ARDU_PIN, INPUT); 107 | 108 | /* Disable all interrupts */ 109 | cli(); 110 | 111 | /* 112 | * ADMUX - ADC Multiplexer Selection Register 113 | * Voltage Reference Selection: AVCC 114 | * ADLAR = 0, (Right adjust) 115 | * MUX[3:0] = 0 by default 116 | */ 117 | ADMUX = _BV(REFS0); 118 | 119 | /* 120 | * ADCSRA - ADC Control and Status Register A 121 | * ADC Enable = 1, 122 | * ADC Start Conversion = 0, 123 | * ADC Auto Trigger Enable = 0, 124 | * ADC Interrupt Flag = 1 (To clear interrupt trigger) 125 | * ADIE: ADC Interrupt Enable = 0, 126 | * ADC Prescaler = 64 (16 Mhz / 64 = 250 KHz) 127 | */ 128 | ADCSRA = (_BV(ADEN) | (_BV(ADPS2) | _BV(ADPS1))); 129 | 130 | /* 131 | * ADCSRB - ADC Control and Status Register B 132 | */ 133 | ADCSRB = 0; 134 | 135 | /* 136 | * DIDR0 - Digital Input Disable Register 0 137 | */ 138 | DIDR0 = ((_BV(ADC_CH0) | _BV(ADC_CH1) | _BV(ADC_CH2) | _BV(ADC_CH3) | _BV(ADC_CH4)) & 0x3F); 139 | 140 | /* Enable all interrupts */ 141 | sei(); 142 | 143 | Uart0_Println(PSTR("[ADC] A6, A1, A2, A3, A7")); 144 | 145 | /* Display current MCU voltage(AVCC) */ 146 | ADC_SetMuxTo1V1(); 147 | Timer1_DelayMillis(5); 148 | Uart0_Println(PSTR("[ADC] AVCC: %f"), ADC_ReadSysVoltage()); 149 | 150 | return 0; 151 | } 152 | 153 | /** 154 | * ADC_Read - Function to trigger ADC single conversion and read ADC value. 155 | * 156 | * Notice! This function require about 57 us execution time. 157 | * 158 | * @param [in] adc_channel Select ADC channel. 159 | * 160 | * @return [uint16_t] digital value of specific ADC channel. 161 | * @retval [0~1023] 162 | * 163 | */ 164 | uint16_t ADC_Read(uint8_t adc_channel) 165 | { 166 | ADC_SetMux(adc_channel); 167 | 168 | /* Return the ADC raw data */ 169 | return ADC_SingleConversion(); 170 | } 171 | 172 | /** 173 | * ADC_SetMuxTo1V1 - 174 | * 175 | * @param [none] 176 | * 177 | * @return [none] 178 | * 179 | */ 180 | void ADC_SetMuxTo1V1() 181 | { 182 | ADC_SetMux(ADC_CH_1V1); 183 | } 184 | 185 | /** 186 | * ADC_ReadSysVoltage - Calculate current voltage of AVCC pin. 187 | * 188 | * We change ADC MUX to 1V1 (bandgap) and compare the voltage difference between AVCC and 1V1. 189 | * 190 | * @param [none] 191 | * 192 | * @return [float] digital value of specific ADC channel. 193 | * @retval [0~N] AVCC voltage 194 | * 195 | */ 196 | float ADC_ReadSysVoltage() 197 | { 198 | /* Assume we already set MUX to 1V1 before, so let's start ADC single conversion directly. */ 199 | return (ADC_BANDGAP_VOLTAGE * 1024.0) / (float)ADC_SingleConversion(); 200 | } 201 | 202 | 203 | /* 204 | ******************************************************************************* 205 | * Private functions declaration 206 | ******************************************************************************* 207 | */ 208 | 209 | /** 210 | * ADC_SetMux - Set ADC MUX to specific channel. 211 | * 212 | * @param [none] 213 | * 214 | * @return [none] 215 | * 216 | */ 217 | static void ADC_SetMux(uint8_t adc_channel) 218 | { 219 | /* Setup MUX for specific channel */ 220 | ADMUX &= ~(_BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0)); 221 | ADMUX |= (adc_channel & (_BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0))); 222 | } 223 | 224 | /** 225 | * ADC_SingleConversion - Start ADC single conversion. 226 | * 227 | * @param [none] 228 | * 229 | * @return [none] 230 | * 231 | */ 232 | static uint16_t ADC_SingleConversion() 233 | { 234 | uint8_t timeout; 235 | 236 | timeout = 0xFF; 237 | 238 | /* ADC start single conversion */ 239 | ADCSRA |= _BV(ADSC); 240 | 241 | /* Wait till single conversion is completed (requires about 57 us) */ 242 | while((ADCSRA & _BV(ADSC))){ 243 | 244 | /* Timeout check, should never expired */ 245 | timeout--; 246 | if(timeout == 0){ 247 | return 0; 248 | } 249 | } 250 | 251 | /* Return the ADC raw data */ 252 | return ADC; 253 | } 254 | 255 | 256 | /* 257 | ******************************************************************************* 258 | * Public functions 259 | ******************************************************************************* 260 | */ 261 | 262 | 263 | /* 264 | ******************************************************************************* 265 | * Private functions 266 | ******************************************************************************* 267 | */ 268 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/adc_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file adc_drv.h 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef ADC_DRV_H_ 17 | #define ADC_DRV_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | #define ADC_CH0 0x06 27 | #define ADC_CH1 0x01 28 | #define ADC_CH2 0x02 29 | #define ADC_CH3 0x03 30 | #define ADC_CH4 0x07 31 | 32 | #define ADC_CH_1V1 0x0E 33 | #define ADC_CH_GND 0x0F 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Data type definition 39 | ******************************************************************************* 40 | */ 41 | 42 | 43 | /* 44 | ******************************************************************************* 45 | * Global variables 46 | ******************************************************************************* 47 | */ 48 | 49 | 50 | /* 51 | ******************************************************************************* 52 | * Public functions declaration 53 | ******************************************************************************* 54 | */ 55 | 56 | 57 | /* 58 | ******************************************************************************* 59 | * Private functions declaration 60 | ******************************************************************************* 61 | */ 62 | 63 | 64 | /* 65 | ******************************************************************************* 66 | * Public functions 67 | ******************************************************************************* 68 | */ 69 | 70 | int8_t ADC_Init(); 71 | uint16_t ADC_Read(uint8_t adc_channel); 72 | void ADC_SetMuxTo1V1(); 73 | float ADC_ReadSysVoltage(); 74 | 75 | 76 | /* 77 | ******************************************************************************* 78 | * Private functions 79 | ******************************************************************************* 80 | */ 81 | 82 | #endif // ADC_DRV_H_ 83 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/ahrs.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file ahrs.h 11 | * @brief AHRS (attitude and heading reference system) 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef AHRS_H_ 17 | #define AHRS_H_ 18 | 19 | #include 20 | #include "imu_ctrl.h" 21 | 22 | 23 | /* 24 | ******************************************************************************* 25 | * Constant value definition 26 | ******************************************************************************* 27 | */ 28 | 29 | /* General */ 30 | #define AHRS_AXES IMU_AXES 31 | #define AHRS_X IMU_X 32 | #define AHRS_Y IMU_Y 33 | #define AHRS_Z IMU_Z 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Data type definition 39 | ******************************************************************************* 40 | */ 41 | 42 | typedef struct ahrs_accel_data{ 43 | int32_t fxp_vctr[AHRS_AXES]; 44 | int16_t sensor_data[AHRS_AXES]; 45 | int32_t G_SQ_FXP; 46 | }AHRS_ACCEL_DATA; 47 | 48 | typedef struct ahrs_ned_attitude{ 49 | float roll_angle; 50 | float pitch_angle; 51 | float heading_angle; 52 | }AHRS_NED_ATTITUDE; 53 | 54 | typedef struct ahrs_body_attitude{ 55 | float roll_angle; 56 | float pitch_angle; 57 | float yaw_angle; 58 | }AHRS_BODY_ATTITUDE; 59 | 60 | typedef struct ahrs_data{ 61 | 62 | /* General */ 63 | uint16_t delta_time; 64 | 65 | /* Accelerometer */ 66 | AHRS_ACCEL_DATA accel; 67 | uint8_t accel_exceed_cnt; 68 | 69 | /* Gyroscope */ 70 | int16_t gyro_sensor_data[AHRS_AXES]; 71 | int32_t gyro_fxp_rads[AHRS_AXES]; 72 | 73 | /* Level vector */ 74 | int32_t level_fxp_vctr[AHRS_AXES]; 75 | 76 | /* Heading vector */ 77 | int32_t heading_fxp_vctr[AHRS_AXES]; 78 | 79 | /* NED Attitude */ 80 | AHRS_NED_ATTITUDE ned_att; 81 | AHRS_BODY_ATTITUDE body_att; 82 | 83 | }AHRS_DATA; 84 | 85 | 86 | /* 87 | ******************************************************************************* 88 | * Global variables 89 | ******************************************************************************* 90 | */ 91 | 92 | 93 | /* 94 | ******************************************************************************* 95 | * Public functions declaration 96 | ******************************************************************************* 97 | */ 98 | 99 | 100 | /* 101 | ******************************************************************************* 102 | * Private functions declaration 103 | ******************************************************************************* 104 | */ 105 | 106 | 107 | /* 108 | ******************************************************************************* 109 | * Public functions 110 | ******************************************************************************* 111 | */ 112 | 113 | int8_t AHRS_Init(AHRS_DATA *p_ahrs, int16_t *p_accel_raw); 114 | int8_t AHRS_AttAngleUpdate(int16_t *p_accel_raw, int16_t *p_gyro_raw, 115 | uint16_t delta_micros, AHRS_DATA *p_ahrs); 116 | 117 | #if defined(IMU_SENSOR_ANGLE_FROM_FG) && IMU_SENSOR_ANGLE_FROM_FG 118 | void AHRS_SetSimAngle(float roll_angle, float pitch_angle, float yaw_angle); 119 | #endif 120 | 121 | 122 | /* 123 | ******************************************************************************* 124 | * Private functions 125 | ******************************************************************************* 126 | */ 127 | 128 | 129 | #endif // AHRS_H_ 130 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/coding_template.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file sensors.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | 17 | /* 18 | ******************************************************************************* 19 | * Constant value definition 20 | ******************************************************************************* 21 | */ 22 | 23 | 24 | /* 25 | ******************************************************************************* 26 | * Data type definition 27 | ******************************************************************************* 28 | */ 29 | 30 | 31 | /* 32 | ******************************************************************************* 33 | * Global variables 34 | ******************************************************************************* 35 | */ 36 | 37 | 38 | /* 39 | ******************************************************************************* 40 | * Public functions declaration 41 | ******************************************************************************* 42 | */ 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Private functions declaration 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Public functions 55 | ******************************************************************************* 56 | */ 57 | 58 | /** 59 | * function_example - Function example 60 | * 61 | * @param [in] input Example input. 62 | * @param [out] *p_output Example output. 63 | * 64 | * @return [int] Function executing result. 65 | * @retval [0] Success. 66 | * @retval [-1] Fail. 67 | * 68 | */ 69 | 70 | 71 | /* 72 | ******************************************************************************* 73 | * Private functions 74 | ******************************************************************************* 75 | */ 76 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/crc_ccitt.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file crc_ccitt.cpp 11 | * 12 | * @brief CRC-16-CCITT (based on Mavlink method): 13 | * - Polynomial = 0x1021 => X^16 + X^12 + X^5 + 1 14 | * - Initial value = 0xFFFF 15 | * - With "reverse data bytes" & "reverse CRC result before final XOR" 16 | * 17 | * https://en.wikipedia.org/wiki/Cyclic_redundancy_check#Commonly_used_and_standardized_CRCs 18 | * https://en.wikipedia.org/wiki/MAVLink 19 | * http://www.sunshine2k.de/coding/javascript/crc/crc_js.html 20 | * ftp://gic.dsc.ulpgc.es/Gic/PFCs/Memoria_TFM_Dima.pdf 21 | * https://github.com/marhar/eastbay-mavlink/blob/master/tests/demo2.py 22 | * http://eastbay-rc.blogspot.tw/2013/04/mavlink-protocol-notes-packet-decoding.html 23 | * 24 | * @author Y.S.Kuo in Hsinchu 25 | ******************************************************************************* 26 | */ 27 | 28 | #include 29 | 30 | #include "crc_ccitt.h" 31 | 32 | 33 | /* 34 | ******************************************************************************* 35 | * Constant value definition 36 | ******************************************************************************* 37 | */ 38 | 39 | 40 | /* 41 | ******************************************************************************* 42 | * Data type definition 43 | ******************************************************************************* 44 | */ 45 | 46 | 47 | /* 48 | ******************************************************************************* 49 | * Global variables 50 | ******************************************************************************* 51 | */ 52 | 53 | 54 | /* 55 | ******************************************************************************* 56 | * Public functions declaration 57 | ******************************************************************************* 58 | */ 59 | 60 | 61 | /* 62 | ******************************************************************************* 63 | * Private functions declaration 64 | ******************************************************************************* 65 | */ 66 | 67 | 68 | /* 69 | ******************************************************************************* 70 | * Public functions 71 | ******************************************************************************* 72 | */ 73 | 74 | /** 75 | * CRC_Accumulate - Function to accumulate CRC of input single byte data. 76 | * (CRC-16-CCITT) 77 | * 78 | * @param [in] data Single byte data 79 | * @param [in] accum_crc Current accumulate CRC value. 80 | * The accum_crc should be initialized to 81 | * CRC_INIT_VAL for first byte data. 82 | * 83 | * @return [uint16_t] CRC value 84 | * 85 | */ 86 | uint16_t CRC_Accumulate(uint8_t data, uint16_t accum_crc) 87 | { 88 | uint8_t tmp; 89 | 90 | tmp = data ^ (uint8_t)(accum_crc & 0xFF); 91 | tmp ^= (tmp << 4); 92 | accum_crc = (accum_crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); 93 | 94 | return accum_crc; 95 | } 96 | 97 | /** 98 | * CRC_Accumulate - Function to accumulate CRC of input multi byte data. 99 | * (CRC-16-CCITT) 100 | * 101 | * @param [in] *p_data Multu byte data. 102 | * @param [in] data_size Size of input data. 103 | * @param [in] accum_crc Current accumulate CRC value. 104 | * The accum_crc should be initialized to 105 | * CRC_INIT_VAL for first byte data. 106 | * 107 | * @return [uint16_t] CRC value 108 | * 109 | */ 110 | uint16_t CRC_AccumulateLoop(uint8_t *p_data, uint8_t data_size, uint16_t accum_crc) 111 | { 112 | while(data_size){ 113 | 114 | accum_crc = CRC_Accumulate(*p_data++, accum_crc); 115 | 116 | data_size--; 117 | } 118 | 119 | return accum_crc; 120 | } 121 | 122 | /** 123 | * CRC_Calculate - Function to calculate CRC (CRC-16-CCITT) of data. 124 | * 125 | * @param [in] *p_data Point to data buffer. 126 | * @param [in] data_size Total byte of input data 127 | * 128 | * @return [uint16_t] CRC value 129 | * 130 | */ 131 | uint16_t CRC_Calculate(uint8_t *p_data, uint8_t data_size) 132 | { 133 | uint16_t crc; 134 | 135 | crc = CRC_INIT_VAL; 136 | 137 | return CRC_AccumulateLoop(p_data, data_size, crc); 138 | } 139 | 140 | 141 | 142 | 143 | /* 144 | ******************************************************************************* 145 | * Private functions 146 | ******************************************************************************* 147 | */ 148 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/crc_ccitt.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file crc_ccitt.h 11 | * 12 | * @brief CRC-16-CCITT (based on Mavlink method): 13 | * 14 | * @author Y.S.Kuo in Hsinchu 15 | ******************************************************************************* 16 | */ 17 | 18 | #ifndef CRC_CCITT_H_ 19 | #define CRC_CCITT_H_ 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | #define CRC_INIT_VAL 0xFFFF 29 | 30 | 31 | /* 32 | ******************************************************************************* 33 | * Data type definition 34 | ******************************************************************************* 35 | */ 36 | 37 | 38 | /* 39 | ******************************************************************************* 40 | * Global variables 41 | ******************************************************************************* 42 | */ 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Public functions declaration 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Private functions declaration 55 | ******************************************************************************* 56 | */ 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Public functions 62 | ******************************************************************************* 63 | */ 64 | 65 | uint16_t CRC_Accumulate(uint8_t data, uint16_t accum_crc); 66 | uint16_t CRC_AccumulateLoop(uint8_t *p_data, uint8_t data_size, uint16_t accum_crc); 67 | uint16_t CRC_Calculate(uint8_t *p_data, uint8_t data_size); 68 | 69 | 70 | 71 | /* 72 | ******************************************************************************* 73 | * Private functions 74 | ******************************************************************************* 75 | */ 76 | 77 | 78 | #endif // CRC_CCITT_H_ 79 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/debug.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file debug.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #include 17 | #include 18 | 19 | #include "debug.h" 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | 29 | /* 30 | ******************************************************************************* 31 | * Data type definition 32 | ******************************************************************************* 33 | */ 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Global variables 39 | ******************************************************************************* 40 | */ 41 | 42 | uint8_t Debug_ISR_NestCnt = 0; 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Public functions declaration 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Private functions declaration 55 | ******************************************************************************* 56 | */ 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Public functions 62 | ******************************************************************************* 63 | */ 64 | 65 | /** 66 | * function_example - Function example 67 | * 68 | * @param [in] input Example input. 69 | * @param [out] *p_output Example output. 70 | * 71 | * @return [int] Function executing result. 72 | * @retval [0] Success. 73 | * @retval [-1] Fail. 74 | * 75 | */ 76 | 77 | 78 | /* 79 | ******************************************************************************* 80 | * Private functions 81 | ******************************************************************************* 82 | */ 83 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/debug.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file debug.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef DEBUG_H_ 17 | #define DEBUG_H_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "timers_drv.h" 24 | 25 | 26 | /* 27 | ******************************************************************************* 28 | * Constant value definition 29 | ******************************************************************************* 30 | */ 31 | 32 | #define DEBUG_ISR_ENABLE false 33 | 34 | extern uint8_t Debug_ISR_NestCnt; 35 | 36 | 37 | #if DEBUG_ISR_ENABLE 38 | 39 | #define DEBUG_ISR_START(vector_num) \ 40 | do{ \ 41 | Debug_ISR_NestCnt++; \ 42 | PORTC |= _BV(PORTC0); \ 43 | }while(0) 44 | 45 | #define DEBUG_ISR_END(vector_num) \ 46 | do{ \ 47 | Debug_ISR_NestCnt--; \ 48 | if(Debug_ISR_NestCnt == 0) \ 49 | PORTC &= ~_BV(PORTC0); \ 50 | }while(0) 51 | 52 | #else 53 | 54 | #define DEBUG_ISR_START(vector_num) 55 | #define DEBUG_ISR_END(vector_num) 56 | 57 | #endif // #if DEBUG_ISR_ENABLE 58 | 59 | /* 60 | ******************************************************************************* 61 | * Data type definition 62 | ******************************************************************************* 63 | */ 64 | 65 | 66 | 67 | /* 68 | ******************************************************************************* 69 | * Global variables 70 | ******************************************************************************* 71 | */ 72 | 73 | 74 | 75 | /* 76 | ******************************************************************************* 77 | * Public functions declaration 78 | ******************************************************************************* 79 | */ 80 | 81 | 82 | /* 83 | ******************************************************************************* 84 | * Private functions declaration 85 | ******************************************************************************* 86 | */ 87 | 88 | 89 | /* 90 | ******************************************************************************* 91 | * Public functions 92 | ******************************************************************************* 93 | */ 94 | 95 | /** 96 | * function_example - Function example 97 | * 98 | * @param [in] input Example input. 99 | * @param [out] *p_output Example output. 100 | * 101 | * @return [int] Function executing result. 102 | * @retval [0] Success. 103 | * @retval [-1] Fail. 104 | * 105 | */ 106 | 107 | 108 | /* 109 | ******************************************************************************* 110 | * Private functions 111 | ******************************************************************************* 112 | */ 113 | 114 | 115 | #endif // DEBUG_H_ 116 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/failsafe.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file failsafe.cpp 11 | * @brief 12 | * 13 | * Hardware: 14 | * WDT 15 | * 16 | * Interrupt: 17 | * ISR(WDT_vect) 18 | * 19 | * @author Y.S.Kuo in Hsinchu 20 | ******************************************************************************* 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "failsafe.h" 30 | #include "rc_in.h" 31 | #include "uart_stream.h" 32 | #include "debug.h" 33 | 34 | 35 | /* 36 | ******************************************************************************* 37 | * Constant value definition 38 | ******************************************************************************* 39 | */ 40 | 41 | 42 | 43 | 44 | /* 45 | ******************************************************************************* 46 | * Data type definition 47 | ******************************************************************************* 48 | */ 49 | 50 | 51 | /* 52 | ******************************************************************************* 53 | * Global variables 54 | ******************************************************************************* 55 | */ 56 | 57 | 58 | /* 59 | ******************************************************************************* 60 | * Public functions declaration 61 | ******************************************************************************* 62 | */ 63 | 64 | 65 | /* 66 | ******************************************************************************* 67 | * Private functions declaration 68 | ******************************************************************************* 69 | */ 70 | 71 | 72 | /* 73 | ******************************************************************************* 74 | * Public functions 75 | ******************************************************************************* 76 | */ 77 | 78 | /** 79 | * FailSafe_Init - Function to initialize failsafe(watchdog) function. 80 | * 81 | * @param [none] 82 | * 83 | * @return [int8_t] Function executing result. 84 | * @retval [0] Success. 85 | * @retval [-1] Fail. 86 | * 87 | */ 88 | int8_t FailSafe_Init() 89 | { 90 | cli(); 91 | wdt_reset(); 92 | 93 | MCUSR &= ~_BV(WDRF); 94 | 95 | /* 96 | * Clear WDT interrupt flag, enable WDT change flag, 97 | * enable WDT interrupt, set to 250ms cycle 98 | */ 99 | WDTCSR = (_BV(WDCE) | _BV(WDE) | _BV(WDIF)); 100 | WDTCSR = (_BV(WDIE) | _BV(WDP2)); 101 | 102 | sei(); 103 | 104 | Uart0_Println(PSTR("[FSafe] WDT: 250ms")); 105 | 106 | return 0; 107 | } 108 | 109 | /** 110 | * FailSafe_Reboot - Function to trig hardware system reset via watchdog. 111 | * 112 | * @param [none] 113 | * 114 | * @return [none] 115 | * 116 | */ 117 | void FailSafe_Reboot() 118 | { 119 | /* Print reset warning before performing system reset*/ 120 | Uart0_Println(PSTR("[FSafe] WDT: rebooting ...")); 121 | 122 | cli(); 123 | wdt_reset(); 124 | 125 | /* 126 | * Set watchdog change enable flag, 127 | * Change WDT timeout to 1 second, and enable system reset mode. 128 | */ 129 | WDTCSR = (_BV(WDCE) | _BV(WDE) | _BV(WDIF)); 130 | WDTCSR = (_BV(WDE) | _BV(WDP1) | _BV(WDP2)); 131 | 132 | /* Wait watchdog trigger and performing HW reset*/ 133 | while(1) 134 | ; 135 | 136 | /* Should never reach here*/ 137 | sei(); 138 | } 139 | 140 | /** 141 | * ISR(WDT_vect) - Watchdog ISR 142 | * 143 | * This function will be called per 250 ms. 144 | * Requires at least 14 us. 145 | * 146 | * @param [none] 147 | * 148 | * @return [none] 149 | * 150 | */ 151 | ISR(WDT_vect) 152 | { 153 | DEBUG_ISR_START(WDT_vect_num); 154 | 155 | /* Be careful, we will enable interrupt temporarily inside RCIN_FailChk function. */ 156 | RCIN_FailChk(); 157 | 158 | DEBUG_ISR_END(WDT_vect_num); 159 | } 160 | 161 | 162 | /* 163 | ******************************************************************************* 164 | * Private functions 165 | ******************************************************************************* 166 | */ 167 | 168 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/failsafe.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file failsafe.h 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef FAILSAFE_H_ 17 | #define FAILSAFE_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | 27 | /* 28 | ******************************************************************************* 29 | * Data type definition 30 | ******************************************************************************* 31 | */ 32 | 33 | 34 | /* 35 | ******************************************************************************* 36 | * Global variables 37 | ******************************************************************************* 38 | */ 39 | 40 | 41 | /* 42 | ******************************************************************************* 43 | * Public functions declaration 44 | ******************************************************************************* 45 | */ 46 | 47 | 48 | /* 49 | ******************************************************************************* 50 | * Private functions declaration 51 | ******************************************************************************* 52 | */ 53 | 54 | 55 | /* 56 | ******************************************************************************* 57 | * Public functions 58 | ******************************************************************************* 59 | */ 60 | 61 | int8_t FailSafe_Init(); 62 | void FailSafe_Reboot(); 63 | 64 | 65 | /* 66 | ******************************************************************************* 67 | * Private functions 68 | ******************************************************************************* 69 | */ 70 | 71 | 72 | #endif // FAILSAFE_H_ 73 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/gps.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file gps.h 11 | * @brief GPS navigation functions based on NMEA-0183. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef GPS_H_ 17 | #define GPS_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * GPS module configuration 25 | ******************************************************************************* 26 | */ 27 | 28 | /* GPS module type definition */ 29 | #define GPS_MODULE_UBLOX6M 30 | 31 | /* Get GPS information from UBLOX NEO-6M module. */ 32 | #if defined(GPS_MODULE_UBLOX6M) 33 | #include "ublox6m_drv.h" 34 | #define GPS_MODULE_NAME UBLOX6M_DEV_NAME 35 | #define GPS_MODULE_INIT() ublox6m_Init() 36 | #define GPS_MODULE_CEP_METERS UBLOX6M_CEP_METERS 37 | 38 | /* Get GPS information from FlightGear FDM. */ 39 | #elif defined(GPS_MODULE_FG) 40 | #define GPS_MODULE_NAME "UNKNOWN" 41 | #define GPS_MODULE_INIT() (0) 42 | #define GPS_MODULE_CEP_METERS (2.5) 43 | 44 | #else 45 | #error "Incorrect GPS module setting" 46 | 47 | #endif 48 | 49 | 50 | /* 51 | ******************************************************************************* 52 | * Constant value definition 53 | ******************************************************************************* 54 | */ 55 | 56 | /* Circular error probable conversion */ 57 | #define GPS_MODULE_RMS_METERS (1.2 * GPS_MODULE_CEP_METERS) /* 63 ~ 68 % */ 58 | #define GPS_MODULE_R95_METERS (2.1 * GPS_MODULE_CEP_METERS) /* 95% */ 59 | #define GPS_MODULE_2DRRMS_METERS (2.4 * GPS_MODULE_CEP_METERS) /* 95 ~ 98% */ 60 | 61 | #define GPS_MOUDLE_DEFAULT_HACCY_METERS (GPS_MODULE_2DRRMS_METERS) 62 | #define GPS_MODILE_RUNTIME_HACCY_METERS(hdop) (hdop * GPS_MODULE_2DRRMS_METERS) 63 | 64 | /* Earth radius, about 6371 km = 6371000 meters */ 65 | #define GPS_EARTH_RADIUS_METERS (6371000) 66 | 67 | #define GPS_KNOTS_TO_M_PER_SEC(knots) ((knots) * 0.5144444) 68 | 69 | #define GPS_FRM_TIMEOUT_MS 2000 70 | 71 | 72 | /* 73 | ******************************************************************************* 74 | * Data type definition 75 | ******************************************************************************* 76 | */ 77 | 78 | /* RX frame decoding state Definition */ 79 | typedef enum gps_rx_nmea_type{ 80 | GPS_RX_NMEA_TYPE_GGA = 0x00, /* Global positioning system fix data */ 81 | GPS_RX_NMEA_TYPE_GLL = 0x10, /* Latitude and longitude, with time of position fix and status */ 82 | GPS_RX_NMEA_TYPE_GSA = 0x20, /* GNSS DOP and Active Satellites */ 83 | GPS_RX_NMEA_TYPE_GSV = 0x30, /* GNSS Satellites in View */ 84 | GPS_RX_NMEA_TYPE_RMC = 0x40, /* Recommended Minimum data */ 85 | GPS_RX_NMEA_TYPE_VTG = 0x50, /* Course over ground and Ground speed */ 86 | GPS_RX_NMEA_TYPE_GRS = 0x60, /* GNSS Range Residuals */ 87 | GPS_RX_NMEA_TYPE_GST = 0x70, /* GNSS Pseudo Range Error Statistics */ 88 | GPS_RX_NMEA_TYPE_ZDA = 0x80, /* Time and Date */ 89 | GPS_RX_NMEA_TYPE_GBS = 0x90, /* GNSS Satellite Fault Detection */ 90 | GPS_RX_NMEA_TYPE_DTM = 0xA0, /* Datum Reference */ 91 | GPS_RX_NMEA_TYPE_THS = 0xB0, /* True Heading and Status */ 92 | GPS_RX_NMEA_TYPE_GPQ = 0xC0, /* Poll message */ 93 | GPS_RX_NMEA_TYPE_TXT = 0xD0, /* Text Transmission */ 94 | GPS_RX_NMEA_TYPE_UNKNOWN = 0xE0, /* Unknown */ 95 | }__attribute__((packed)) GPS_RX_NMEA_TYPE; 96 | 97 | /* Coordinate data structure */ 98 | typedef struct gps_coord_point{ 99 | float LAT_DD; /* Latitude in decimal degrees format. */ 100 | float LONG_DD; /* Longitude in decimal degrees format. */ 101 | }GPS_COORD_POINT; 102 | 103 | /* Data structure to store NMEA report information */ 104 | typedef struct gps_nmea_report{ 105 | struct{ 106 | float UTC; /* UTC, hhmmdd.sss. */ 107 | GPS_COORD_POINT coord; /* Current coordinate in decimal degrees format. */ 108 | uint8_t fix_status; /* fix status. */ 109 | uint8_t SAT_Used; /* total using satellite. */ 110 | float HDOP; /* Horizontal Dilution of Precision. */ 111 | float ALT_meters; /* Altitude in meters. */ 112 | }gpgga; 113 | 114 | struct{ 115 | uint8_t nav_status; /* Navigation status. */ 116 | float gnd_speed_MS; /* Ground speed, meter/second. */ 117 | float COG_degrees; /* Course over ground, 0~ 360 degree. */ 118 | uint32_t date; /* Current data, YYMMDD. */ 119 | uint8_t fix_status; /* fix status, NMEA version 2.3 only. */ 120 | }gprmc; 121 | 122 | }GPS_NMEA_REPORT; 123 | 124 | /* Waypoint information data structure */ 125 | typedef struct gps_waypoint_data{ 126 | 127 | bool is_set; /* Indicator to show whether waypoint is set or not */ 128 | bool is_valid; /* Indicator to show whether current waypoint information is valid or not */ 129 | uint8_t discard_cnt; /* Total discarded NMEA frame (based on CEP check) */ 130 | uint8_t update_cycle; /* Update cycle counter */ 131 | GPS_COORD_POINT coord; /* Waypoint coordinate */ 132 | float bearing_angle; /* True bearing angle between current position and waypoint */ 133 | float distance; /* Distance between current position and waypoint */ 134 | 135 | }GPS_WAYPOINT_DATA; 136 | 137 | /* Navigation information data structure */ 138 | typedef struct gps_navigation_data{ 139 | 140 | bool is_position_set; /* Flag to indicate whether the current position is initialized or not */ 141 | GPS_COORD_POINT current_coord; /* Current position of vehicle. */ 142 | 143 | bool is_valid; /* Indicator to show whether current navigation information is valid or not */ 144 | float relative_bearing_angle; /* Relative bearing angle between current moving direction and waypoint */ 145 | 146 | }GPS_NAVIGATION_DATA; 147 | 148 | /* GPS data */ 149 | typedef struct gps_data{ 150 | 151 | struct{ 152 | 153 | uint8_t gga_det_cnt; /* NMEA GPGGA frame detected count */ 154 | uint8_t gga_invalid_cnt; /* NMEA GPGGA frame invalid count */ 155 | uint32_t gga_timestamp; /* NMEA GPGGA frame RX timestamp */ 156 | 157 | uint8_t rmc_det_cnt; /* NMEA GPRMC frame detected count */ 158 | uint8_t rmc_invalid_cnt; /* NMEA GPRMC frame invalid count */ 159 | uint32_t rmc_timestamp; /* NMEA GPRMC frame RX timestamp */ 160 | 161 | uint8_t uknown_det_cnt; /* NMEA unknown frame detected count */ 162 | uint32_t uknown_timestamp; /* NMEA unknown frame RX timestamp */ 163 | 164 | }general; 165 | 166 | GPS_NMEA_REPORT nmea; 167 | 168 | GPS_WAYPOINT_DATA wpt; 169 | 170 | GPS_NAVIGATION_DATA nav; 171 | 172 | }GPS_DATA; 173 | 174 | /* GPS RX error log */ 175 | typedef struct gps_error_log{ 176 | uint8_t nmea_field_err_cnt; 177 | uint8_t nmea_chksum_err_cnt; 178 | uint8_t nmea_end_err_cnt; 179 | uint8_t rx_timeout_cnt; 180 | }GPS_ERROR_LOG; 181 | 182 | 183 | /* 184 | ******************************************************************************* 185 | * Global variables 186 | ******************************************************************************* 187 | */ 188 | 189 | extern GPS_ERROR_LOG GPS_ErrorLog; 190 | 191 | 192 | /* 193 | ******************************************************************************* 194 | * Public functions declaration 195 | ******************************************************************************* 196 | */ 197 | 198 | 199 | /* 200 | ******************************************************************************* 201 | * Private functions declaration 202 | ******************************************************************************* 203 | */ 204 | 205 | 206 | /* 207 | ******************************************************************************* 208 | * Public functions 209 | ******************************************************************************* 210 | */ 211 | 212 | int8_t GPS_Init(GPS_DATA *p_gps_data); 213 | GPS_RX_NMEA_TYPE GPS_UpdateNMEA(GPS_DATA *p_gps_data); 214 | int8_t GPS_UpdateNav(GPS_DATA *p_gps_data); 215 | 216 | int8_t GPS_SetWpt(GPS_DATA *p_gps_data, GPS_COORD_POINT *p_waypoint); 217 | int8_t GPS_ClrWpt(GPS_DATA *p_gps_data); 218 | int8_t GPS_GetWptDistance(GPS_DATA *p_gps_data, float *p_distance); 219 | int8_t GPS_GetWptTrueBearing(GPS_DATA *p_gps_data, float *p_bearing); 220 | float GPS_GetWptRelativeBearing(GPS_DATA *p_gps_data); 221 | 222 | float GPS_CalInitTrueBearingAngle(GPS_COORD_POINT *p_src, GPS_COORD_POINT *p_dest); 223 | float GPS_CalApproxDistance(GPS_COORD_POINT *p_src, GPS_COORD_POINT *p_dest); 224 | 225 | 226 | /* 227 | ******************************************************************************* 228 | * Private functions 229 | ******************************************************************************* 230 | */ 231 | 232 | 233 | #endif // GPS_H_ 234 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/i2c_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file i2c_drv.h 11 | * @brief AVR TWI (I2C) driver functions. 12 | * Support repeat start and burst read/write mode. 13 | * 14 | * Reference: Atmel 328P datasheet 15 | * Table 22-2. Status codes for Master Transmitter Mode. 16 | * Figure 22-10. Interfacing the Application to the TWI in a Typical 17 | * Transmission. 18 | * 19 | * @author Y.S.Kuo in Hsinchu 20 | ******************************************************************************* 21 | */ 22 | 23 | #ifndef I2C_DRV_H_ 24 | #define I2C_DRV_H_ 25 | 26 | #include 27 | 28 | 29 | /* 30 | ******************************************************************************* 31 | * Constant value definition 32 | ******************************************************************************* 33 | */ 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Data type definition 39 | ******************************************************************************* 40 | */ 41 | 42 | 43 | /* 44 | ******************************************************************************* 45 | * Global variables 46 | ******************************************************************************* 47 | */ 48 | 49 | 50 | /* 51 | ******************************************************************************* 52 | * Public functions declaration 53 | ******************************************************************************* 54 | */ 55 | 56 | int8_t I2C_Init(uint32_t i2c_hz); 57 | int8_t I2C_WriteBytes(uint8_t dev_id, uint8_t reg_addr, 58 | uint8_t bytes, uint8_t *p_data); 59 | int8_t I2C_WriteByte(uint8_t dev_id, uint8_t reg_addr, uint8_t data); 60 | int8_t I2C_ReadBytes(uint8_t dev_id, uint8_t reg_addr, 61 | uint8_t bytes, uint8_t *p_data); 62 | int8_t I2C_ReadByte(uint8_t dev_id, uint8_t reg_addr, uint8_t *p_data); 63 | 64 | #endif // I2C_DRV_H_ 65 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/imu_ctrl.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file imu_ctrl.h 11 | * @brief IMU (inertial measurement unit) 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef IMU_CTRL_H_ 17 | #define IMU_CTRL_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * IMU sensors configuration 25 | ******************************************************************************* 26 | */ 27 | 28 | /* IMU sensor hardware type definition */ 29 | #define IMU_SENSOR_MPU6050 30 | 31 | /* General */ 32 | #define IMU_AXES 3 33 | #define IMU_X 0 34 | #define IMU_Y 1 35 | #define IMU_Z 2 36 | 37 | 38 | /* 39 | ******************************************************************************* 40 | * Constant value definition 41 | ******************************************************************************* 42 | */ 43 | 44 | /* Get IMU information from MPU6050 real sensor. */ 45 | #if defined(IMU_SENSOR_MPU6050) 46 | #include "mpu6050_drv.h" 47 | #define IMU_SENSOR_NAME "MPU6050" 48 | #define IMU_SENSOR_UNIT_1G MPU6050_UNIT_1G 49 | #define IMU_SENSOR_UNIT_1DPS MPU6050_UNIT_1DPS 50 | #define IMU_SENSOR_CAL_MODE IMU_SENSOR_CAL_RUNTIME 51 | #define IMU_SENSOR_CAL_ACCEL_DEF {113, -63, -59} 52 | #define IMU_SENSOR_CAL_GYRO_DEF {-27, 11, 9} 53 | #define IMU_SENSOR_INIT() mpu6050_Init() 54 | #define IMU_SENSOR_GET_6_RAW_DATA(p_accel, p_gyro) mpu6050_ReadXYZDirectly(p_accel, p_gyro) 55 | 56 | /* Get IMU information from PC off-line data. */ 57 | #elif defined(IMU_SENSOR_PC_SIM) 58 | #define IMU_SENSOR_PC_EN true 59 | #define IMU_SENSOR_NAME "IMU_PC_SIM" 60 | #define IMU_SENSOR_UNIT_1G (4096) 61 | #define IMU_SENSOR_UNIT_1DPS (16.4) 62 | #define IMU_SENSOR_CAL_MODE IMU_SENSOR_CAL_NONE 63 | #define IMU_SENSOR_CAL_ACCEL_DEF {0, 0, 0} 64 | #define IMU_SENSOR_CAL_GYRO_DEF {0, 0, 0} 65 | #define IMU_SENSOR_INIT() IMU_SIM_Init() 66 | #define IMU_SENSOR_GET_6_RAW_DATA(p_accel, p_gyro) IMU_SIM_Get6RawData(p_accel, p_gyro) 67 | #define IMU_SENSOR_SAMPLE_TIME IMU_SIM_SampleMicros 68 | 69 | /* Get IMU information from FlightGear FDM. */ 70 | #elif defined(IMU_SENSOR_FG_SIM) 71 | #define IMU_SENSOR_FG_EN true 72 | #define IMU_SENSOR_NAME "IMU_FG_SIM" 73 | #define IMU_SENSOR_ANGLE_FROM_FG true /* Get NED angle from FG directly */ 74 | #define IMU_SENSOR_UNIT_1G (4096) 75 | #define IMU_SENSOR_UNIT_1DPS (16.4) 76 | #define IMU_SENSOR_CAL_MODE IMU_SENSOR_CAL_NONE 77 | #define IMU_SENSOR_CAL_ACCEL_DEF {0, 0, 0} 78 | #define IMU_SENSOR_CAL_GYRO_DEF {0, 0, 0} 79 | #define IMU_SENSOR_INIT() IMU_FG_Init() 80 | #define IMU_SENSOR_GET_6_RAW_DATA(p_accel, p_gyro) IMU_FG_Get6RawData(p_accel, p_gyro) 81 | #define IMU_SENSOR_UPDATE_FROM_UART(p_accel, p_gyro) IMU_FG_UpdateIMUFromUart(p_accel, p_gyro) 82 | 83 | #else 84 | #error "Incorrect IMU sensor setting" 85 | 86 | #endif 87 | 88 | 89 | /* 90 | ******************************************************************************* 91 | * Data type definition 92 | ******************************************************************************* 93 | */ 94 | 95 | /* IMU calibration mode definition */ 96 | typedef enum imu_sensor_op{ 97 | 98 | IMU_SENSOR_CAL_PROG_DEF = 0, /* Apply code default calibrated bias. */ 99 | IMU_SENSOR_CAL_RUNTIME, /* Perform calibration procedure now. */ 100 | IMU_SENSOR_CAL_NONE, /* Do NOT apply any calibrated bias. */ 101 | }IMU_SENSOR_CAL_OP; 102 | 103 | /* IMU sensors raw data */ 104 | typedef struct imu_sensor_data{ 105 | int16_t accel_raw[IMU_AXES]; 106 | int16_t gyro_raw[IMU_AXES]; 107 | }IMU_SENSOR_DATA; 108 | 109 | 110 | /* 111 | ******************************************************************************* 112 | * Global variables 113 | ******************************************************************************* 114 | */ 115 | 116 | #if defined(IMU_SENSOR_PC_EN) 117 | extern uint16_t IMU_SIM_SampleMicros; 118 | #endif 119 | 120 | 121 | /* 122 | ******************************************************************************* 123 | * Public functions declaration 124 | ******************************************************************************* 125 | */ 126 | 127 | 128 | /* 129 | ******************************************************************************* 130 | * Private functions declaration 131 | ******************************************************************************* 132 | */ 133 | 134 | 135 | /* 136 | ******************************************************************************* 137 | * Public functions 138 | ******************************************************************************* 139 | */ 140 | 141 | int8_t IMU_Init(); 142 | int8_t IMU_Get6RawData(IMU_SENSOR_DATA *p_data); 143 | int8_t IMU_DoCalibration(IMU_SENSOR_CAL_OP cal_mode); 144 | void IMU_SetCalibratedBias(int16_t *p_accel_bias, int16_t *p_gyro_bias); 145 | void IMU_GetCalibratedBias(int16_t *p_accel_bias, int16_t *p_gyro_bias); 146 | 147 | #if defined(IMU_SENSOR_FG_EN) 148 | void IMU_FG_UpdateIMUFromUart(int16_t *p_accel_xyz, int16_t *p_gyro_xyz); 149 | #endif 150 | 151 | 152 | /* 153 | ******************************************************************************* 154 | * Private functions 155 | ******************************************************************************* 156 | */ 157 | 158 | #endif // IMU_CTRL_H_ 159 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/leds_ctrl.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file leds_ctrl.cpp 11 | * @brief LED controlling functions. 12 | * 13 | * LED/Pin/ mapping: 14 | * Master LED = Arduino D13 / AVR PB5 15 | * Slave LED = Arduino D12 / AVR PB4 16 | * 17 | * @author Y.S.Kuo in Hsinchu 18 | ******************************************************************************* 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "leds_ctrl.h" 26 | #include "timers_drv.h" 27 | #include "uart_stream.h" 28 | 29 | 30 | /* 31 | ******************************************************************************* 32 | * Constant value definition 33 | ******************************************************************************* 34 | */ 35 | 36 | 37 | /* 38 | ******************************************************************************* 39 | * Data type definition 40 | ******************************************************************************* 41 | */ 42 | 43 | typedef enum led_state{ 44 | LED_NORMAL_OFF_STATE = 0, 45 | LED_NORMAL_ON_STATE, 46 | LED_LIGHTING_ON_STATE, 47 | LED_LIGHTING_OFF_STATE, 48 | }LED_STATE; 49 | 50 | typedef struct led{ 51 | uint8_t ardu_pin; 52 | volatile uint8_t *p_pin_reg; 53 | uint8_t reg_bit; 54 | LED_STATE led_state; 55 | uint32_t prev_time; 56 | }LED; 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Global variables 62 | ******************************************************************************* 63 | */ 64 | 65 | LED LEDS_Data[LEDS_TOTAL] = 66 | { 67 | [LEDS_MASTER_IDX] = {13, &PORTB, _BV(PORTB5), LED_NORMAL_OFF_STATE, 0}, /* Master, Arduino pin 13, PB5 */ 68 | [LEDS_SLAVE_IDX] = {14, &PORTC, _BV(PORTC0), LED_NORMAL_OFF_STATE, 0}, /* Slave, Arduino pin 14(A0), PB4 */ 69 | }; 70 | 71 | 72 | /* 73 | ******************************************************************************* 74 | * Public functions declaration 75 | ******************************************************************************* 76 | */ 77 | 78 | /** 79 | * LEDS_Init - Function to initialize LED pins and power OFF LED by default. 80 | * 81 | * @param [none] 82 | * 83 | * @return [none] 84 | * 85 | */ 86 | void LEDS_Init() 87 | { 88 | uint8_t led_idx; 89 | 90 | Uart0_Printf(PSTR("[LEDS] Pins:")); 91 | 92 | /* Initial ports */ 93 | for(led_idx = 0; led_idx < LEDS_TOTAL; led_idx++){ 94 | pinMode(LEDS_Data[led_idx].ardu_pin, OUTPUT); 95 | LEDS_PwrOFF(led_idx); 96 | LEDS_Data[led_idx].prev_time = Timer1_GetMillis(); 97 | 98 | Uart0_Printf(PSTR(" %hhu"), LEDS_Data[led_idx].ardu_pin); 99 | } 100 | 101 | Uart0_Println(PSTR("")); 102 | } 103 | 104 | /** 105 | * LEDS_PwrON - Function to power ON specific LED. 106 | * 107 | * @param [in] led_idx Index of specific LED. 108 | * Refer to enum LEDS_IDX{...}. 109 | * 110 | * @return [int8_t] Function executing result. 111 | * @retval [0] Success. 112 | * @retval [-1] Fail. 113 | * 114 | */ 115 | int8_t LEDS_PwrON(uint8_t led_idx) 116 | { 117 | if(led_idx >= LEDS_TOTAL) 118 | return -1; 119 | 120 | *(LEDS_Data[led_idx].p_pin_reg) |= LEDS_Data[led_idx].reg_bit; 121 | 122 | return 0; 123 | } 124 | 125 | /** 126 | * LEDS_PwrOFF - Function to power OFF specific LED. 127 | * 128 | * @param [in] led_idx Index of specific LED. 129 | * Refer to enum LEDS_IDX{...}. 130 | * 131 | * @return [int8_t] Function executing result. 132 | * @retval [0] Success. 133 | * @retval [-1] Fail. 134 | * 135 | */ 136 | int8_t LEDS_PwrOFF(uint8_t led_idx) 137 | { 138 | if(led_idx >= LEDS_TOTAL) 139 | return -1; 140 | 141 | *(LEDS_Data[led_idx].p_pin_reg) &= ~(LEDS_Data[led_idx].reg_bit); 142 | 143 | return 0; 144 | } 145 | 146 | /** 147 | * LEDS_Blink - Function to blink specific LED. 148 | * 149 | * @param [in] led_idx Index of specific LED. 150 | * Refer to enum LEDS_IDX{...}. 151 | * 152 | * @param [in] on_millis ON period setting in millisecond. 153 | * 154 | * @param [in] off_millis OFF period setting in millisecond. 155 | * 156 | * @return [int8_t] Function executing result. 157 | * @retval [0] Success. 158 | * @retval [-1] Fail. 159 | * 160 | */ 161 | int8_t LEDS_Blink(uint8_t led_idx, uint32_t on_millis, uint32_t off_millis) 162 | { 163 | uint32_t delta_time; 164 | LED *p_led; 165 | 166 | if(led_idx >= LEDS_TOTAL) 167 | return -1; 168 | 169 | p_led = &LEDS_Data[led_idx]; 170 | 171 | delta_time = Timer1_GetMillis() - p_led->prev_time; 172 | 173 | /* ON state */ 174 | if(p_led->led_state == LED_NORMAL_ON_STATE){ 175 | 176 | if(delta_time >= on_millis){ 177 | *(p_led->p_pin_reg) &= ~(p_led->reg_bit); 178 | p_led->led_state = LED_NORMAL_OFF_STATE; 179 | p_led->prev_time = Timer1_GetMillis(); 180 | } 181 | 182 | } 183 | /* OFF state */ 184 | else{ 185 | if(delta_time >= off_millis){ 186 | *(p_led->p_pin_reg) |= (p_led->reg_bit); 187 | p_led->led_state = LED_NORMAL_ON_STATE; 188 | p_led->prev_time = Timer1_GetMillis(); 189 | } 190 | } 191 | 192 | return 0; 193 | } 194 | 195 | /** 196 | * LEDS_Lightning - Function to blink specific LED like lightning. 197 | * 198 | * @param [in] led_idx Index of specific LED. 199 | * Refer to enum LEDS_IDX{...}. 200 | * 201 | * @param [in] off_millis OFF period setting in millisecond. 202 | * 203 | * @param [in] lighting_on_millis lighting ON period setting in millisecond. 204 | * @param [in] lighting_off_millis lighting OFF period setting in millisecond. 205 | * 206 | * 207 | * @return [int8_t] Function executing result. 208 | * @retval [0] Success. 209 | * @retval [-1] Fail. 210 | * 211 | */ 212 | int8_t LEDS_Lightning(uint8_t led_idx, uint32_t off_millis, 213 | uint32_t lighting_on_millis, uint32_t lighting_off_millis) 214 | { 215 | uint32_t delta_time; 216 | LED *p_led; 217 | 218 | if(led_idx >= LEDS_TOTAL) 219 | return -1; 220 | 221 | p_led = &LEDS_Data[led_idx]; 222 | 223 | delta_time = Timer1_GetMillis() - p_led->prev_time; 224 | 225 | switch(p_led->led_state){ 226 | case LED_NORMAL_OFF_STATE: 227 | 228 | /* Turn ON LED if needed */ 229 | if(delta_time >= off_millis){ 230 | *(p_led->p_pin_reg) |= (p_led->reg_bit); 231 | p_led->led_state = LED_NORMAL_ON_STATE; 232 | p_led->prev_time = Timer1_GetMillis(); 233 | } 234 | 235 | break; 236 | case LED_NORMAL_ON_STATE: 237 | 238 | /* Turn OFF LED if needed */ 239 | if(delta_time >= lighting_on_millis){ 240 | *(p_led->p_pin_reg) &= ~(p_led->reg_bit); 241 | p_led->led_state = LED_LIGHTING_OFF_STATE; 242 | p_led->prev_time = Timer1_GetMillis(); 243 | } 244 | 245 | break; 246 | case LED_LIGHTING_OFF_STATE: 247 | 248 | /* Turn ON LED if needed */ 249 | if(delta_time >= lighting_off_millis){ 250 | *(p_led->p_pin_reg) |= (p_led->reg_bit); 251 | p_led->led_state = LED_LIGHTING_ON_STATE; 252 | p_led->prev_time = Timer1_GetMillis(); 253 | } 254 | 255 | break; 256 | case LED_LIGHTING_ON_STATE: 257 | 258 | /* Turn ON LED if needed */ 259 | if(delta_time >= lighting_on_millis){ 260 | *(p_led->p_pin_reg) &= ~(p_led->reg_bit); 261 | p_led->led_state = LED_NORMAL_OFF_STATE; 262 | p_led->prev_time = Timer1_GetMillis(); 263 | } 264 | 265 | break; 266 | default: 267 | break; 268 | } 269 | 270 | return 0; 271 | } 272 | 273 | 274 | /* 275 | ******************************************************************************* 276 | * Private functions declaration 277 | ******************************************************************************* 278 | */ 279 | 280 | 281 | /* 282 | ******************************************************************************* 283 | * Public functions 284 | ******************************************************************************* 285 | */ 286 | 287 | /** 288 | * function_example - Function example 289 | * 290 | * @param [in] input Example input. 291 | * @param [out] *p_output Example output. 292 | * 293 | * @return [int] Function executing result. 294 | * @retval [0] Success. 295 | * @retval [-1] Fail. 296 | * 297 | */ 298 | 299 | 300 | /* 301 | ******************************************************************************* 302 | * Private functions 303 | ******************************************************************************* 304 | */ 305 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/leds_ctrl.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file leds_ctrl.h 11 | * @brief LED controlling functions. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef LEDS_CTRL_H_ 17 | #define LEDS_CTRL_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | #define LEDS_TOTAL 2 27 | 28 | 29 | /* 30 | ******************************************************************************* 31 | * Data type definition 32 | ******************************************************************************* 33 | */ 34 | 35 | enum LEDS_IDX{ 36 | LEDS_MASTER_IDX = 0, 37 | LEDS_SLAVE_IDX, 38 | }; 39 | 40 | 41 | /* 42 | ******************************************************************************* 43 | * Public functions declaration 44 | ******************************************************************************* 45 | */ 46 | 47 | void LEDS_Init(); 48 | int8_t LEDS_PwrON(uint8_t led_idx); 49 | int8_t LEDS_PwrOFF(uint8_t led_idx); 50 | int8_t LEDS_Blink(uint8_t led_idx, uint32_t on_millis, uint32_t off_millis); 51 | int8_t LEDS_Lightning(uint8_t led_idx, uint32_t off_millis, 52 | uint32_t lighting_on_millis, uint32_t lighting_off_millis); 53 | 54 | 55 | /* 56 | ******************************************************************************* 57 | * Private functions 58 | ******************************************************************************* 59 | */ 60 | 61 | 62 | #endif // LEDS_CTRL_H_ 63 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/math_lib.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file math_lib.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef MATH_LIB_H_ 17 | #define MATH_LIB_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | #define MATH_PI (3.14159265359) 29 | #define MATH_PI_HALF (1.570796326795) 30 | 31 | #define MATH_DEG2RAD(deg) ((deg) * (MATH_PI / 180.0)) 32 | #define MATH_RAD2DEG(rad) ((rad) * (180 / MATH_PI)) 33 | 34 | #define MATH_MAX(a,b) ((a) > (b) ? a : b) 35 | #define MATH_MIN(a,b) ((a) < (b) ? a : b) 36 | 37 | 38 | /* 39 | ******************************************************************************* 40 | * Data type definition 41 | ******************************************************************************* 42 | */ 43 | 44 | 45 | /* 46 | ******************************************************************************* 47 | * Global variables 48 | ******************************************************************************* 49 | */ 50 | 51 | 52 | /* 53 | ******************************************************************************* 54 | * Public functions declaration 55 | ******************************************************************************* 56 | */ 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Private functions declaration 62 | ******************************************************************************* 63 | */ 64 | 65 | 66 | /* 67 | ******************************************************************************* 68 | * Public functions 69 | ******************************************************************************* 70 | */ 71 | 72 | float Math_FastSqrt(float val); 73 | float Math_FastInvSqrt(float x); 74 | float Math_Atan2Nvidia(float y, float x); 75 | float Math_Atan2Approx1(float y, float x); 76 | float Math_Atan2Approx2(float y, float x); 77 | float Math_FastSin(float x); 78 | float Math_FastCos(float x); 79 | float Math_FastSin2(float x); 80 | float Math_FastCos2(float x); 81 | 82 | 83 | /* 84 | ******************************************************************************* 85 | * Private functions 86 | ******************************************************************************* 87 | */ 88 | 89 | 90 | #endif // MATH_LIB_H_ 91 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/mcu_protocol.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file mcu_protocol.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #include 17 | 18 | #include "mcu_protocol.h" 19 | #include "uart_drv.h" 20 | #include "uart_stream.h" 21 | #include "crc_ccitt.h" 22 | 23 | 24 | /* 25 | ******************************************************************************* 26 | * Constant value definition 27 | ******************************************************************************* 28 | */ 29 | 30 | 31 | /* 32 | ******************************************************************************* 33 | * Data type definition 34 | ******************************************************************************* 35 | */ 36 | 37 | /* RX frame decoding state Definition */ 38 | typedef enum mp_rx_frm_state{ 39 | MP_RX_FRM_WAIT_SFLAG = 0, 40 | MP_RX_FRM_WAIT_HDR, 41 | MP_RX_FRM_WAIT_PAYLOAD, 42 | MP_RX_FRM_WAIT_CRC, 43 | }MP_RX_FRM_STATE; 44 | 45 | 46 | /* 47 | ******************************************************************************* 48 | * Macros definition 49 | ******************************************************************************* 50 | */ 51 | 52 | 53 | /* 54 | ******************************************************************************* 55 | * Global variables 56 | ******************************************************************************* 57 | */ 58 | 59 | static uint8_t MP_TxSequence; 60 | 61 | /* The following variables are created for storing information of receiving frame */ 62 | static MP_RX_FRM_STATE MP_RxFrmState; /* Current RX frame function state */ 63 | static uint8_t MP_RxSequence; /* Current RX frame sequence */ 64 | static uint8_t MP_RxFrmBufIdx; /* Current write index of RX buffer */ 65 | static uint8_t MP_RxFrmBuf[MP_RX_FRM_BUF_SIZE]; /* RX frame buffer */ 66 | static uint8_t MP_RxFrmPayloadLen; /* Expected payload length of RX frame */ 67 | static uint8_t MP_RxFrmCrcIdx; /* Total collected CRC byte */ 68 | static uint16_t MP_RxFrmCrc16; /* Expected RX frame checksum */ 69 | static bool MP_RXFrmIsLost; /* Frame lost indicator */ 70 | 71 | 72 | /* 73 | ******************************************************************************* 74 | * Public functions declaration 75 | ******************************************************************************* 76 | */ 77 | 78 | 79 | /* 80 | ******************************************************************************* 81 | * Private functions declaration 82 | ******************************************************************************* 83 | */ 84 | 85 | 86 | /* 87 | ******************************************************************************* 88 | * Public functions 89 | ******************************************************************************* 90 | */ 91 | 92 | /** 93 | * MP_Init - Function to initialize MCU protocol. 94 | * 95 | * @param [none] 96 | * 97 | * @return [int8_t] Function executing result. 98 | * @retval [0] Success. 99 | * @retval [-1] Fail. 100 | * 101 | */ 102 | int8_t MP_Init() 103 | { 104 | MP_TxSequence = 0; 105 | 106 | MP_RxSequence = 0; 107 | MP_RxFrmBufIdx = 0; 108 | MP_RxFrmPayloadLen = 0; 109 | MP_RxFrmCrcIdx = 0; 110 | MP_RxFrmCrc16 = CRC_INIT_VAL; 111 | MP_RxFrmState = MP_RX_FRM_WAIT_SFLAG; 112 | MP_RXFrmIsLost = false; 113 | memset((void *)MP_RxFrmBuf, 0, sizeof(MP_RxFrmBuf)); 114 | 115 | Uart0_Println(PSTR("[MP] OK")); 116 | 117 | return 0; 118 | } 119 | 120 | /** 121 | * MP_Send - Function to send MP frame. 122 | * 123 | * @param [in] cmd MP command ID (Refer to enum mp_rsp_cmd). 124 | * @param [in] *p_data Frame data buffer. 125 | * @param [in] data_size Size of frame data. 126 | * 127 | * @return [uint8_t] Transmitted data size. 128 | * @retval [0] Fail. 129 | * @retval [1~255] Transmitted bytes. 130 | * 131 | */ 132 | uint8_t MP_Send(uint8_t cmd, uint8_t *p_data, uint8_t data_size) 133 | { 134 | uint16_t tx_bytes; 135 | uint16_t crc; 136 | MP_FRAME_HDR hdr; 137 | MP_FRAME_TAIL tail; 138 | 139 | if(p_data == NULL || data_size == 0) 140 | return 0; 141 | 142 | if(sizeof(hdr) + data_size + sizeof(tail) > 255) 143 | return 0; 144 | 145 | /* Header */ 146 | hdr.s_flag = MP_FRM_SFLAG; 147 | hdr.cmd = cmd; 148 | hdr.sequence = MP_TxSequence++; 149 | hdr.len = data_size; 150 | 151 | /* Tail */ 152 | crc = CRC_INIT_VAL; 153 | crc = CRC_AccumulateLoop(&hdr.cmd, sizeof(hdr) - sizeof(hdr.s_flag), crc); 154 | crc = CRC_AccumulateLoop(p_data, data_size, crc); 155 | tail.CRC16 = crc; 156 | 157 | tx_bytes = 0; 158 | 159 | tx_bytes += Uart0_WriteBytes((uint8_t *)&hdr, sizeof(hdr)); 160 | tx_bytes += Uart0_WriteBytes(p_data, data_size); 161 | tx_bytes += Uart0_WriteBytes((uint8_t *)&tail, sizeof(tail)); 162 | 163 | return tx_bytes; 164 | } 165 | 166 | /** 167 | * MP_Recv - Function to receive MP frame. 168 | * 169 | * This function should be called periodically in the system loop. 170 | * 171 | * @param [in/out] *p_frm_buf A buffer to store received MP frame content. 172 | * @param [in] frm_buf_size Byte size of input frame buffer. 173 | * 174 | * @return [uint8_t] Total received frame size. 175 | * @retval [0] No RX frame. 176 | * @retval [1~N] Byte size of received MP frame (Header + Payload + Checksum). 177 | * 178 | */ 179 | uint8_t MP_Recv(uint8_t *p_frm_buf, uint8_t frm_buf_size) 180 | { 181 | uint8_t current_rx_cnt; 182 | uint8_t total_frm_size; 183 | uint8_t data_byte; 184 | MP_FRAME_HDR *p_frm_hdr; 185 | MP_FRAME_TAIL *p_frm_tail; 186 | 187 | current_rx_cnt = 0; 188 | total_frm_size = 0; 189 | 190 | /* 191 | * Process received byte, but break this loop once we received numbers of 192 | * frame data in case the keep comping data cause endless loop. 193 | */ 194 | while(Uart0_ReadByte(&data_byte) && current_rx_cnt < MP_RX_FRM_BUF_SIZE){ 195 | 196 | /* Drop all collected frame data if the frame length is larger than our local buffer size */ 197 | if(MP_RxFrmBufIdx == MP_RX_FRM_BUF_SIZE) 198 | MP_RxFrmState = MP_RX_FRM_WAIT_SFLAG; 199 | 200 | switch(MP_RxFrmState){ 201 | 202 | /* Detecting frame state flag */ 203 | case MP_RX_FRM_WAIT_SFLAG: 204 | 205 | MP_RxFrmBufIdx = 0; 206 | MP_RxFrmPayloadLen = 0; 207 | MP_RxFrmCrcIdx = 0; 208 | MP_RxFrmCrc16 = CRC_INIT_VAL; 209 | 210 | if(data_byte == MP_FRM_SFLAG){ 211 | 212 | /* Store frame data in local buffer */ 213 | MP_RxFrmBuf[MP_RxFrmBufIdx] = data_byte; 214 | MP_RxFrmBufIdx++; 215 | 216 | MP_RxFrmState = MP_RX_FRM_WAIT_HDR; 217 | } 218 | 219 | break; 220 | 221 | /* Collecting frame header */ 222 | case MP_RX_FRM_WAIT_HDR: 223 | 224 | /* Store frame data in local buffer */ 225 | MP_RxFrmBuf[MP_RxFrmBufIdx] = data_byte; 226 | MP_RxFrmBufIdx++; 227 | 228 | MP_RxFrmCrc16 = CRC_Accumulate(data_byte, MP_RxFrmCrc16); 229 | 230 | if(MP_RxFrmBufIdx == sizeof(MP_FRAME_HDR)){ 231 | p_frm_hdr = (MP_FRAME_HDR *)MP_RxFrmBuf; 232 | 233 | MP_RxFrmPayloadLen = p_frm_hdr->len; 234 | 235 | if(MP_RxSequence != p_frm_hdr->sequence) 236 | MP_RXFrmIsLost = true; 237 | 238 | MP_RxSequence = p_frm_hdr->sequence; 239 | 240 | if(p_frm_hdr->len == 0) 241 | MP_RxFrmState = MP_RX_FRM_WAIT_CRC; 242 | else 243 | MP_RxFrmState = MP_RX_FRM_WAIT_PAYLOAD; 244 | } 245 | 246 | break; 247 | 248 | /* Collecting frame payload */ 249 | case MP_RX_FRM_WAIT_PAYLOAD: 250 | 251 | /* Store frame data in local buffer */ 252 | MP_RxFrmBuf[MP_RxFrmBufIdx] = data_byte; 253 | MP_RxFrmBufIdx++; 254 | MP_RxFrmPayloadLen--; 255 | 256 | MP_RxFrmCrc16 = CRC_Accumulate(data_byte, MP_RxFrmCrc16); 257 | 258 | if(MP_RxFrmPayloadLen == 0){ 259 | MP_RxFrmState = MP_RX_FRM_WAIT_CRC; 260 | } 261 | 262 | break; 263 | 264 | case MP_RX_FRM_WAIT_CRC: 265 | 266 | /* Store frame data in local buffer */ 267 | MP_RxFrmBuf[MP_RxFrmBufIdx] = data_byte; 268 | MP_RxFrmBufIdx++; 269 | MP_RxFrmCrcIdx++; 270 | 271 | if(MP_RxFrmCrcIdx == sizeof(((MP_FRAME_TAIL *)0)->CRC16)){ 272 | 273 | p_frm_tail = (MP_FRAME_TAIL *)&MP_RxFrmBuf[MP_RxFrmBufIdx - MP_RxFrmCrcIdx]; 274 | 275 | if(p_frm_tail->CRC16 == MP_RxFrmCrc16){ 276 | 277 | /* Copy frame data to external buffer */ 278 | if(p_frm_buf != NULL && frm_buf_size >= MP_RxFrmBufIdx){ 279 | total_frm_size = MP_RxFrmBufIdx; 280 | memcpy((void *)p_frm_buf, MP_RxFrmBuf, total_frm_size); 281 | } 282 | 283 | } 284 | 285 | MP_RxFrmState = MP_RX_FRM_WAIT_SFLAG; 286 | } 287 | 288 | break; 289 | 290 | default: 291 | break; 292 | } 293 | 294 | if(total_frm_size != 0) 295 | break; 296 | 297 | current_rx_cnt++; 298 | } 299 | 300 | return total_frm_size; 301 | } 302 | 303 | 304 | /* 305 | ******************************************************************************* 306 | * Private functions 307 | ******************************************************************************* 308 | */ 309 | 310 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/mcu_protocol.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file mcu_protocol.cpp 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef MCU_PROTOCOL_H_ 17 | #define MCU_PROTOCOL_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | #define MP_FRM_SFLAG 0x7E /* Frame start flag */ 29 | 30 | #define MP_TX_FRM_BUF_SIZE 128 31 | #define MP_RX_FRM_BUF_SIZE 64 32 | 33 | 34 | /* 35 | ******************************************************************************* 36 | * Data type definition 37 | ******************************************************************************* 38 | */ 39 | 40 | /* MP request command (For host side) */ 41 | typedef enum mp_req_cmd{ 42 | /* General */ 43 | MP_REQ_SYS_HEARTBEAT = 0, 44 | MP_REQ_SYS_GENERAL, 45 | MP_REQ_SYS_SETPOINT, 46 | MP_REQ_SYS_CRUISE_STATE, 47 | MP_REQ_SYS_RESERVED = 31, 48 | 49 | /* GPS */ 50 | MP_REQ_GPS_DATA = 32, 51 | MP_REQ_GPS_GENERAL, 52 | MP_REQ_GPS_NMEA_FULL, 53 | MP_REQ_GPS_NMEA_GGA, 54 | MP_REQ_GPS_NMEA_RMC, 55 | MP_REQ_GPS_WAYPOINT, 56 | MP_REQ_GPS_NAVIGATION, 57 | MP_REQ_GPS_ERR_LOG, 58 | 59 | /* IMU and AHRS */ 60 | MP_REQ_IMU_SENSOR_DATA = 64, 61 | MP_REQ_NED_ANGLE_DATA, 62 | 63 | MP_REQ_AHRS_FULL = 72, 64 | MP_REQ_AHRS_ACCEL, 65 | 66 | /* PID */ 67 | MP_REQ_PID_DATA_ROLL = 80, 68 | MP_REQ_PID_VAL_ROLL, 69 | MP_REQ_PID_CFG_ROLL, 70 | 71 | MP_REQ_PID_DATA_PITCH, 72 | MP_REQ_PID_VAL_PITCH, 73 | MP_REQ_PID_CFG_PITCH, 74 | 75 | MP_REQ_PID_DATA_YAW, 76 | MP_REQ_PID_VAL_YAW, 77 | MP_REQ_PID_CFG_YAW, 78 | 79 | MP_REQ_PID_DATA_BANK, 80 | MP_REQ_PID_VAL_BANK, 81 | MP_REQ_PID_CFG_BANK, 82 | 83 | /* RC control */ 84 | MP_REQ_IN_CHANNELS = 96, 85 | MP_REQ_OUT_CHANNELS = 112, 86 | 87 | /* For normal ASCII message */ 88 | MP_REQ_ASCII_MSG = 120, 89 | 90 | /* Never use */ 91 | MP_REQ_END = 127 92 | }MP_REQ_CMD; 93 | 94 | /* MP response command (For MCU side) */ 95 | typedef enum mp_rsp_cmd{ 96 | /* General */ 97 | MP_RSP_SYS_HEARTBEAT = MP_REQ_SYS_HEARTBEAT + 128, 98 | MP_RSP_SYS_GENERAL = MP_REQ_SYS_GENERAL + 128, 99 | MP_RSP_SYS_SETPOINT = MP_REQ_SYS_SETPOINT + 128, 100 | MP_RSP_SYS_CRUISE_STATE = MP_REQ_SYS_CRUISE_STATE + 128, 101 | MP_RSP_SYS_RESERVED = MP_REQ_SYS_RESERVED + 128, 102 | 103 | /* GPS */ 104 | MP_RSP_GPS_DATA = MP_REQ_GPS_DATA + 128, 105 | MP_RSP_GPS_GENERAL = MP_REQ_GPS_GENERAL + 128, 106 | MP_RSP_GPS_NMEA_FULL = MP_REQ_GPS_NMEA_FULL + 128, 107 | MP_RSP_GPS_NMEA_GGA = MP_REQ_GPS_NMEA_GGA + 128, 108 | MP_RSP_GPS_NMEA_RMC = MP_REQ_GPS_NMEA_RMC + 128, 109 | MP_RSP_GPS_WAYPOINT = MP_REQ_GPS_WAYPOINT + 128, 110 | MP_RSP_GPS_NAVIGATION = MP_REQ_GPS_NAVIGATION + 128, 111 | MP_RSP_GPS_ERR_LOG = MP_REQ_GPS_ERR_LOG + 128, 112 | 113 | /* IMU */ 114 | MP_RSP_IMU_SENSOR_DATA = MP_REQ_IMU_SENSOR_DATA + 128, 115 | MP_RSP_NED_ANGLE_DATA, 116 | 117 | /* AHRS */ 118 | MP_RSP_AHRS_FULL = MP_REQ_AHRS_FULL + 128, 119 | MP_RSP_AHRS_ACCEL, 120 | 121 | /* PID */ 122 | MP_RSP_PID_DATA_ROLL = MP_REQ_PID_DATA_ROLL + 128, 123 | MP_RSP_PID_VAL_ROLL, 124 | MP_RSP_PID_CFG_ROLL, 125 | 126 | MP_RSP_PID_DATA_PITCH, 127 | MP_RSP_PID_VAL_PITCH, 128 | MP_RSP_PID_CFG_PITCH, 129 | 130 | MP_RSP_PID_DATA_YAW, 131 | MP_RSP_PID_VAL_YAW, 132 | MP_RSP_PID_CFG_YAW, 133 | 134 | MP_RSP_PID_DATA_BANK, 135 | MP_RSP_PID_VAL_BANK, 136 | MP_RSP_PID_CFG_BANK, 137 | 138 | /* RC control */ 139 | MP_RSP_IN_CHANNELS = MP_REQ_IN_CHANNELS + 128, 140 | MP_RSP_OUT_CHANNELS = MP_REQ_OUT_CHANNELS + 128, 141 | 142 | /* For normal ASCII message */ 143 | MP_RSP_ASCII_MSG = MP_REQ_ASCII_MSG + 128, 144 | 145 | /* Never use */ 146 | MP_RSP_END = MP_REQ_END + 128 147 | }MP_RSP_CMD; 148 | 149 | /* Frame header of MCU protocol */ 150 | typedef struct mp_frame_hdr{ 151 | uint8_t s_flag; /* 0x7E */ 152 | uint8_t cmd; /* Command */ 153 | uint8_t sequence; /* Sequence number */ 154 | uint8_t len; /* Length of payload not including checksum */ 155 | uint8_t payload[0]; /* Payload */ 156 | }__attribute__((packed)) MP_FRAME_HDR; 157 | 158 | /* Frame tail of MCU protocol */ 159 | typedef struct mp_frame_tail{ 160 | uint16_t CRC16; /* 16 bits CRC (CRC-16-CCITT) appended after data */ 161 | }__attribute__((packed)) MP_FRAME_TAIL; 162 | 163 | 164 | /* 165 | ******************************************************************************* 166 | * Global variables 167 | ******************************************************************************* 168 | */ 169 | 170 | 171 | /* 172 | ******************************************************************************* 173 | * Public functions declaration 174 | ******************************************************************************* 175 | */ 176 | 177 | 178 | /* 179 | ******************************************************************************* 180 | * Private functions declaration 181 | ******************************************************************************* 182 | */ 183 | 184 | 185 | /* 186 | ******************************************************************************* 187 | * Public functions 188 | ******************************************************************************* 189 | */ 190 | 191 | int8_t MP_Init(); 192 | uint8_t MP_Send(uint8_t cmd, uint8_t *p_data, uint8_t data_size); 193 | uint8_t MP_Recv(uint8_t *p_frm_buf, uint8_t frm_buf_size); 194 | 195 | 196 | /* 197 | ******************************************************************************* 198 | * Private functions 199 | ******************************************************************************* 200 | */ 201 | 202 | 203 | #endif // MCU_PROTOCOL_H_ 204 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/pid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file pid.cpp 11 | * @brief PID controller (proportional integral derivative controller) 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #include 17 | 18 | #include "pid.h" 19 | 20 | 21 | /* 22 | ******************************************************************************* 23 | * Constant value definition 24 | ******************************************************************************* 25 | */ 26 | 27 | 28 | /* 29 | ******************************************************************************* 30 | * Data type definition 31 | ******************************************************************************* 32 | */ 33 | 34 | 35 | /* 36 | ******************************************************************************* 37 | * Global variables 38 | ******************************************************************************* 39 | */ 40 | 41 | 42 | /* 43 | ******************************************************************************* 44 | * Public functions declaration 45 | ******************************************************************************* 46 | */ 47 | 48 | 49 | /* 50 | ******************************************************************************* 51 | * Private functions declaration 52 | ******************************************************************************* 53 | */ 54 | 55 | 56 | /* 57 | ******************************************************************************* 58 | * Public functions 59 | ******************************************************************************* 60 | */ 61 | 62 | /** 63 | * PID_Create - Function to create new PID controller. 64 | * 65 | * @param [in] *p_pid Core data of PID controller which 66 | * will be reset by this function. 67 | * 68 | * @return [none] 69 | * 70 | */ 71 | void PID_Create(PID_DATA *p_pid) 72 | { 73 | /* DO NOT use memset to reset float variable */ 74 | p_pid->config.KP = 0; 75 | p_pid->config.KI = 0; 76 | p_pid->config.KD = 0; 77 | p_pid->config.scale_factor = 1.0; 78 | p_pid->config.output_max = 0; 79 | p_pid->config.integral_max = 0; 80 | 81 | p_pid->value.integral = 0; 82 | p_pid->value.derivative = 0; 83 | p_pid->value.delta_time = 0; 84 | p_pid->value.prev_error = 0; 85 | p_pid->value.output = 0; 86 | } 87 | 88 | /** 89 | * PID_SetTuning - Function to set KP, KI, and KD gain of specific 90 | * PID controller. 91 | * 92 | * @param [out] *p_pid Core data of specific PID controller which 93 | * will be updated by this function. 94 | * 95 | * @param [in] KP New proportional gain setting. 96 | * @param [in] KI New integral gain setting. 97 | * @param [in] KD New derivative gain setting. 98 | * 99 | * @return [none] 100 | * 101 | */ 102 | void PID_SetTuning(PID_DATA *p_pid, float KP, float KI, float KD) 103 | { 104 | p_pid->config.KP = KP; 105 | p_pid->config.KI = KI; 106 | p_pid->config.KD = KD; 107 | } 108 | 109 | /** 110 | * PID_SetScaleFactor - Function to set scale factor of specific PID controller. 111 | * 112 | * @param [out] *p_pid Core data of specific PID controller which 113 | * will be updated by this function. 114 | * 115 | * @param [in] scale_factor New scale factor setting. 116 | * 117 | * @return [none] 118 | * 119 | */ 120 | void PID_SetScaleFactor(PID_DATA *p_pid, float scale_factor) 121 | { 122 | p_pid->config.scale_factor = scale_factor; 123 | } 124 | 125 | /** 126 | * PID_SetIntegralMax - Function to set maximum/minimum integral value of 127 | * specific PID controller. 128 | * (integral_max >= integral value >= -integral_max) 129 | * 130 | * @param [out] *p_pid Core data of specific PID controller which 131 | * will be updated by this function. 132 | * 133 | * @param [in] integral_max MAX/MIN integral threshold setting. 134 | * 135 | * @return [none] 136 | * 137 | */ 138 | void PID_SetIntegralMax(PID_DATA *p_pid, float integral_max) 139 | { 140 | p_pid->config.integral_max = integral_max; 141 | } 142 | 143 | /** 144 | * PID_SetOutputMax - Function to set maximum/minimum PID output value of 145 | * specific PID controller. 146 | * (output_max >= output >= -output_max) 147 | * 148 | * @param [out] *p_pid Core data of specific PID controller which 149 | * will be updated by this function. 150 | * 151 | * @param [in] output_max MAX/MIN output threshold setting. 152 | * 153 | * @return [none] 154 | * 155 | */ 156 | void PID_SetOutputMax(PID_DATA *p_pid, float output_max) 157 | { 158 | p_pid->config.output_max = output_max; 159 | } 160 | 161 | /** 162 | * PID_Reset - Function to reset PID controller. 163 | * 164 | * @param [out] *p_pid Core data of specific PID controller which 165 | * will be updated by this function. 166 | * 167 | * @return [none] 168 | * 169 | */ 170 | void PID_Reset(PID_DATA *p_pid) 171 | { 172 | p_pid->value.prev_error = 0; 173 | p_pid->value.derivative = 0; 174 | p_pid->value.integral = 0; 175 | } 176 | 177 | /** 178 | * PID_Update - Function to perform PID calculation and generate the ideal 179 | * output control value according latest ERROR input and related 180 | * gain setting of specific PID controller. 181 | * 182 | * @param [out] *p_pid Core data of specific PID controller which 183 | * will be updated by this function. 184 | * 185 | * @param [in] error The latest measured error. 186 | * @param [in] delta_time The PID delta time. 187 | * @param [in] is_integral_en Set turn to integrate input error. 188 | * 189 | * @return [float] Ideal control output which is decided by PID controller. 190 | * 191 | */ 192 | float PID_Update(PID_DATA *p_pid, float error, uint16_t delta_time, bool is_integral_en) 193 | { 194 | float delta_t; 195 | float scale_factor; 196 | float error_tmp; 197 | 198 | scale_factor = p_pid->config.scale_factor; 199 | delta_t = delta_time * 0.000001; 200 | 201 | /* Rescale the error if need */ 202 | error_tmp = error * scale_factor; 203 | 204 | /* Calculate derivative */ 205 | p_pid->value.derivative = ((error - p_pid->value.prev_error) / delta_t) * scale_factor; 206 | 207 | /* Calculate integral if needed */ 208 | if(is_integral_en == true){ 209 | p_pid->value.integral += (error * delta_t) * scale_factor; 210 | } 211 | 212 | /* Check integral range */ 213 | if(p_pid->value.integral > p_pid->config.integral_max) 214 | p_pid->value.integral = p_pid->config.integral_max; 215 | else if(p_pid->value.integral < -(p_pid->config.integral_max)) 216 | p_pid->value.integral = -(p_pid->config.integral_max); 217 | 218 | /* PID update */ 219 | p_pid->value.output = error_tmp * p_pid->config.KP 220 | + p_pid->value.integral * p_pid->config.KI 221 | + p_pid->value.derivative * p_pid->config.KD; 222 | 223 | /* Check output range */ 224 | if(p_pid->value.output > p_pid->config.output_max) 225 | p_pid->value.output = p_pid->config.output_max; 226 | else if(p_pid->value.output < -(p_pid->config.output_max)) 227 | p_pid->value.output = -(p_pid->config.output_max); 228 | 229 | p_pid->value.prev_error = error; 230 | p_pid->value.delta_time = delta_time; 231 | 232 | return p_pid->value.output; 233 | } 234 | 235 | 236 | /* 237 | ******************************************************************************* 238 | * Private functions 239 | ******************************************************************************* 240 | */ 241 | 242 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/pid.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file pid.cpp 11 | * @brief PID controller (proportional integral derivative controller) 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef PID_H_ 17 | #define PID_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | 29 | /* 30 | ******************************************************************************* 31 | * Data type definition 32 | ******************************************************************************* 33 | */ 34 | 35 | typedef struct pid_value{ 36 | float integral; 37 | float derivative; 38 | float output; 39 | float prev_error; 40 | uint16_t delta_time; 41 | }PID_VALUE; 42 | 43 | typedef struct pid_config{ 44 | float KP; 45 | float KI; 46 | float KD; 47 | float scale_factor; 48 | 49 | float integral_max; 50 | float output_max; 51 | }PID_CONFIG; 52 | 53 | typedef struct pid_data{ 54 | PID_VALUE value; 55 | PID_CONFIG config; 56 | }PID_DATA; 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Global variables 62 | ******************************************************************************* 63 | */ 64 | 65 | 66 | /* 67 | ******************************************************************************* 68 | * Public functions declaration 69 | ******************************************************************************* 70 | */ 71 | 72 | 73 | /* 74 | ******************************************************************************* 75 | * Private functions declaration 76 | ******************************************************************************* 77 | */ 78 | 79 | 80 | /* 81 | ******************************************************************************* 82 | * Public functions 83 | ******************************************************************************* 84 | */ 85 | 86 | void PID_Create(PID_DATA *p_pid); 87 | void PID_SetTuning(PID_DATA *p_pid, float KP, float KI, float KD); 88 | void PID_SetScaleFactor(PID_DATA *p_pid, float scale_factor); 89 | void PID_SetIntegralMax(PID_DATA *p_pid, float integral_max); 90 | void PID_SetOutputMax(PID_DATA *p_pid, float output_max); 91 | void PID_Reset(PID_DATA *p_pid); 92 | float PID_Update(PID_DATA *p_pid, float error, uint16_t delta_time, bool is_integral_en); 93 | 94 | 95 | /* 96 | ******************************************************************************* 97 | * Private functions 98 | ******************************************************************************* 99 | */ 100 | 101 | 102 | #endif // PID_H_ 103 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/pin_change.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file pin_change.cpp 11 | * @brief Input pin change initialization function and interrupt handler. 12 | * 13 | * Hardware: 14 | * Pin change detector. 15 | * 16 | * Interrupt: 17 | * ISR(PCINT0_vect) 18 | * ISR(PCINT1_vect) 19 | * ISR(PCINT2_vect) 20 | * 21 | * 22 | * @author Y.S.Kuo in Hsinchu 23 | ******************************************************************************* 24 | */ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "pin_change.h" 32 | #include "timers_drv.h" 33 | #include "uart_stream.h" 34 | #include "uart_sim.h" 35 | #include "rc_in.h" 36 | #include "debug.h" 37 | 38 | 39 | /* 40 | ******************************************************************************* 41 | * Constant value definition 42 | ******************************************************************************* 43 | */ 44 | 45 | 46 | /* 47 | ******************************************************************************* 48 | * Data type definition 49 | ******************************************************************************* 50 | */ 51 | 52 | static uint8_t PC_PrevPinState[PC_GRP_TOTAL]; 53 | 54 | 55 | /* 56 | ******************************************************************************* 57 | * Global variables 58 | ******************************************************************************* 59 | */ 60 | 61 | 62 | /* 63 | ******************************************************************************* 64 | * Public functions declaration 65 | ******************************************************************************* 66 | */ 67 | 68 | 69 | 70 | /* 71 | ******************************************************************************* 72 | * Private functions declaration 73 | ******************************************************************************* 74 | */ 75 | 76 | 77 | /* 78 | ******************************************************************************* 79 | * Public functions 80 | ******************************************************************************* 81 | */ 82 | 83 | /** 84 | * PC_Init - Function to initialize pin change detection functions. 85 | * 86 | * We disable all PC functions by default. 87 | * 88 | * @param [none] 89 | * 90 | * @return [int8_t] Function executing result. 91 | * @retval [0] Success. 92 | * @retval [-1] Fail. 93 | * 94 | */ 95 | int8_t PC_Init() 96 | { 97 | /* Disable global interrupt */ 98 | cli(); 99 | 100 | memset((void *)PC_PrevPinState, 0, sizeof(PC_PrevPinState)); 101 | 102 | /* Disable external interrupt request */ 103 | EIMSK = 0x00; 104 | 105 | /* Clear pin change mask (Disable all PC interrupts) */ 106 | PCMSK0 = 0; 107 | PCMSK1 = 0; 108 | PCMSK2 = 0; 109 | 110 | /* Disable pin change interrupt (group 0~2) */ 111 | PCICR &= ~(_BV(PCIF0) | _BV(PCIF1) | _BV(PCIF2)); 112 | 113 | /* Enable global interrupt */ 114 | sei(); 115 | 116 | Uart0_Println(PSTR("[PinChg] OK")); 117 | 118 | return 0; 119 | } 120 | 121 | /** 122 | * PC_Setup - Function to enable/disable specific PC pin interrupt. 123 | * 124 | * We disable all PC functions by default. 125 | * 126 | * @param [input] pc_grp_idx Group index of specific PC pin. 127 | * @param [input] pc_pin_mask Bitmask of specific PC pin. 128 | * @param [input] is_enable Set true to enable corresponding PC interrupt. 129 | * 130 | * @return [int8_t] Function executing result. 131 | * @retval [0] Success. 132 | * @retval [-1] Fail. 133 | * 134 | */ 135 | int8_t PC_Setup(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask, bool is_enable) 136 | { 137 | volatile uint8_t pc_mask; 138 | 139 | /* Setup PCMSKx register */ 140 | if(is_enable){ 141 | PC_QuickEnable(pc_grp_idx, pc_pin_mask); 142 | } 143 | else{ 144 | PC_QuickDisable(pc_grp_idx, pc_pin_mask); 145 | } 146 | 147 | switch(pc_grp_idx){ 148 | case PC_GRP_IDX_0: 149 | 150 | pc_mask = PCMSK0; 151 | 152 | break; 153 | 154 | case PC_GRP_IDX_1: 155 | 156 | pc_mask = PCMSK1; 157 | 158 | break; 159 | 160 | case PC_GRP_IDX_2: 161 | 162 | pc_mask = PCMSK2; 163 | 164 | break; 165 | 166 | default: 167 | pc_mask = 0; 168 | 169 | break; 170 | } 171 | 172 | /* Setup PCICR register */ 173 | if(pc_mask) 174 | PCICR |= _BV(pc_grp_idx); 175 | else 176 | PCICR &= ~_BV(pc_grp_idx); 177 | 178 | return 0; 179 | } 180 | 181 | /** 182 | * PC_QuickEnable - Function to quick enable specific PC pin interrupt. 183 | * 184 | * Please call PC_Setup() one time first for any specific PC pin. 185 | * 186 | * @param [input] pc_grp_idx Group index of specific PC pin. 187 | * @param [input] pc_pin_mask Bitmask of specific PC pin. 188 | * 189 | * @return [none] 190 | * 191 | */ 192 | void PC_QuickEnable(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask) 193 | { 194 | switch(pc_grp_idx){ 195 | case PC_GRP_IDX_0: 196 | 197 | PCMSK0 |= pc_pin_mask; 198 | 199 | break; 200 | 201 | case PC_GRP_IDX_1: 202 | 203 | PCMSK1 |= pc_pin_mask; 204 | 205 | break; 206 | 207 | case PC_GRP_IDX_2: 208 | 209 | PCMSK2 |= pc_pin_mask; 210 | 211 | break; 212 | 213 | default: 214 | 215 | break; 216 | } 217 | } 218 | 219 | /** 220 | * PC_QuickDisable - Function to quick disable specific PC pin interrupt. 221 | * 222 | * Please call PC_Setup() one time first for any specific PC pin. 223 | * 224 | * @param [input] pc_grp_idx Group index of specific PC pin. 225 | * @param [input] pc_pin_mask Bitmask of specific PC pin. 226 | * 227 | * @return [none] 228 | * 229 | */ 230 | void PC_QuickDisable(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask) 231 | { 232 | switch(pc_grp_idx){ 233 | case PC_GRP_IDX_0: 234 | 235 | PCMSK0 &= ~pc_pin_mask; 236 | 237 | break; 238 | 239 | case PC_GRP_IDX_1: 240 | 241 | PCMSK1 &= ~pc_pin_mask; 242 | 243 | break; 244 | 245 | case PC_GRP_IDX_2: 246 | 247 | PCMSK2 &= ~pc_pin_mask; 248 | 249 | break; 250 | 251 | default: 252 | break; 253 | } 254 | } 255 | 256 | 257 | /* 258 | ******************************************************************************* 259 | * Private functions 260 | ******************************************************************************* 261 | */ 262 | 263 | /** 264 | * ISR(PCINT0_vect) - Pin change group 0 ISR 265 | * 266 | * @param [none] 267 | * 268 | * @return [none] 269 | * 270 | */ 271 | ISR(PCINT0_vect) 272 | { 273 | const PC_GRP_IDX grp_idx = PC_GRP_IDX_0; 274 | uint32_t trig_time = Timer1_GetTicks32(); 275 | uint8_t pin_status = PC_GRP_0_IO_VALUE(); 276 | uint8_t pin_change = PC_PrevPinState[grp_idx] ^ pin_status; 277 | 278 | DEBUG_ISR_START(PCINT0_vect_num); 279 | 280 | PC_PrevPinState[grp_idx] = pin_status; 281 | 282 | /* Be carefully, we will enable interrupt temporarily inside RCIN_PulseHandler function. */ 283 | RCIN_PulseHandler(grp_idx, trig_time, pin_status, pin_change); 284 | 285 | DEBUG_ISR_END(PCINT0_vect_num); 286 | } 287 | 288 | /** 289 | * ISR(PCINT1_vect) - Pin change group 1 ISR 290 | * 291 | * @param [none] 292 | * 293 | * @return [none] 294 | * 295 | */ 296 | ISR(PCINT1_vect) 297 | { 298 | const PC_GRP_IDX grp_idx = PC_GRP_IDX_1; 299 | uint32_t trig_time = Timer1_GetTicks32(); 300 | uint8_t pin_status = PC_GRP_1_IO_VALUE(); 301 | uint8_t pin_change = PC_PrevPinState[grp_idx] ^ pin_status; 302 | 303 | DEBUG_ISR_START(PCINT1_vect_num); 304 | 305 | PC_PrevPinState[grp_idx] = pin_status; 306 | 307 | /* 308 | * void handler(grp_idx, grp_shift, trig_time, pin_change) 309 | * { 310 | * } 311 | */ 312 | 313 | DEBUG_ISR_END(PCINT1_vect_num); 314 | } 315 | 316 | /** 317 | * ISR(PCINT2_vect) - Pin change group 2 ISR 318 | * 319 | * @param [none] 320 | * 321 | * @return [none] 322 | * 323 | */ 324 | ISR(PCINT2_vect) 325 | { 326 | const PC_GRP_IDX grp_idx = PC_GRP_IDX_2; 327 | uint32_t trig_time = Timer1_GetTicks32(); 328 | uint8_t pin_status = PC_GRP_2_IO_VALUE(); 329 | uint8_t pin_change = PC_PrevPinState[grp_idx] ^ pin_status; 330 | 331 | DEBUG_ISR_START(PCINT2_vect_num); 332 | 333 | PC_PrevPinState[grp_idx] = pin_status; 334 | 335 | #if UARTS_FUNCTION_EN 336 | /* Check simulated UART RX pin */ 337 | UartS_RxPulseHandler(grp_idx, trig_time, pin_status, pin_change); 338 | #endif 339 | 340 | /* Be carefully, we will enable interrupt temporarily inside RCIN_PulseHandler function. */ 341 | RCIN_PulseHandler(grp_idx, trig_time, pin_status, pin_change); 342 | 343 | DEBUG_ISR_END(PCINT2_vect_num); 344 | } 345 | 346 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/pin_change.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file pin_change.h 11 | * @brief Input pin change initialization function and interrupt handler. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef PIN_CHANGE_H_ 17 | #define PIN_CHANGE_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | #define PC_GRP_0_IO_VALUE() PINB 27 | #define PC_GRP_1_IO_VALUE() PINC 28 | #define PC_GRP_2_IO_VALUE() PIND 29 | 30 | 31 | /* 32 | ******************************************************************************* 33 | * Data type definition 34 | ******************************************************************************* 35 | */ 36 | 37 | 38 | typedef enum pc_pin_idx{ 39 | /* Pin change group 0 */ 40 | PC_PIN_IDX_0 = 0, /* PortB 0 */ 41 | PC_PIN_IDX_1, /* PortB 1 */ 42 | PC_PIN_IDX_2, /* PortB 2 */ 43 | PC_PIN_IDX_3, /* PortB 3 */ 44 | PC_PIN_IDX_4, /* PortB 4 */ 45 | PC_PIN_IDX_5, /* PortB 5 */ 46 | PC_PIN_IDX_6, /* PortB 6 */ 47 | PC_PIN_IDX_7, /* PortB 7 */ 48 | 49 | /* Pin change group 1 */ 50 | PC_PIN_IDX_8, /* PortC 0 */ 51 | PC_PIN_IDX_9, /* PortC 1 */ 52 | PC_PIN_IDX_10, /* PortC 2 */ 53 | PC_PIN_IDX_11, /* PortC 3 */ 54 | PC_PIN_IDX_12, /* PortC 4 */ 55 | PC_PIN_IDX_13, /* PortC 5 */ 56 | PC_PIN_IDX_14, /* PortC 6 */ 57 | PC_PIN_IDX_UNUSED0, /* Unused 0 */ 58 | 59 | /* Pin change group 2 */ 60 | PC_PIN_IDX_16, /* PortD 0 */ 61 | PC_PIN_IDX_17, /* PortD 1 */ 62 | PC_PIN_IDX_18, /* PortD 2 */ 63 | PC_PIN_IDX_19, /* PortD 3 */ 64 | PC_PIN_IDX_20, /* PortD 4 */ 65 | PC_PIN_IDX_21, /* PortD 5 */ 66 | PC_PIN_IDX_22, /* PortD 6 */ 67 | PC_PIN_IDX_23, /* PortD 7 */ 68 | }__attribute__((packed)) PC_PIN_IDX; 69 | 70 | typedef enum pc_pin_mask{ 71 | /* Pin change group 0 */ 72 | PC_PIN_MASK_0 = _BV(PCINT0), /* PortB 0 */ 73 | PC_PIN_MASK_1 = _BV(PCINT1), /* PortB 1 */ 74 | PC_PIN_MASK_2 = _BV(PCINT2), /* PortB 2 */ 75 | PC_PIN_MASK_3 = _BV(PCINT3), /* PortB 3 */ 76 | PC_PIN_MASK_4 = _BV(PCINT4), /* PortB 4 */ 77 | PC_PIN_MASK_5 = _BV(PCINT5), /* PortB 5 */ 78 | PC_PIN_MASK_6 = _BV(PCINT6), /* PortB 6 */ 79 | PC_PIN_MASK_7 = _BV(PCINT7), /* PortB 7 */ 80 | 81 | /* Pin change group 1 */ 82 | PC_PIN_MASK_8 = _BV(PCINT8), /* PortC 0 */ 83 | PC_PIN_MASK_9 = _BV(PCINT9), /* PortC 1 */ 84 | PC_PIN_MASK_10 = _BV(PCINT10), /* PortC 2 */ 85 | PC_PIN_MASK_11 = _BV(PCINT11), /* PortC 3 */ 86 | PC_PIN_MASK_12 = _BV(PCINT12), /* PortC 4 */ 87 | PC_PIN_MASK_13 = _BV(PCINT13), /* PortC 5 */ 88 | PC_PIN_MASK_14 = _BV(PCINT14), /* PortC 6 */ 89 | PC_PIN_MASK_UNUSED0 = 0, /* Unused 0 */ 90 | 91 | /* Pin change group 2 */ 92 | PC_PIN_MASK_16 = _BV(PCINT16), /* PortD 0 */ 93 | PC_PIN_MASK_17 = _BV(PCINT17), /* PortD 1 */ 94 | PC_PIN_MASK_18 = _BV(PCINT18), /* PortD 2 */ 95 | PC_PIN_MASK_19 = _BV(PCINT19), /* PortD 3 */ 96 | PC_PIN_MASK_20 = _BV(PCINT20), /* PortD 4 */ 97 | PC_PIN_MASK_21 = _BV(PCINT21), /* PortD 5 */ 98 | PC_PIN_MASK_22 = _BV(PCINT22), /* PortD 6 */ 99 | PC_PIN_MASK_23 = _BV(PCINT23), /* PortD 7 */ 100 | }__attribute__((packed)) PC_PIN_MASK; 101 | 102 | typedef enum pc_grp_idx{ 103 | PC_GRP_IDX_0 = 0, /* Pin group 0, Port B */ 104 | PC_GRP_IDX_1, /* Pin group 1, Port C */ 105 | PC_GRP_IDX_2, /* Pin group 2, Port D */ 106 | PC_GRP_TOTAL = 3, 107 | }__attribute__((packed)) PC_GRP_IDX; 108 | 109 | typedef enum pc_grp_shift{ 110 | PC_GRP_SHIFT_0 = 0, 111 | PC_GRP_SHIFT_1 = 8, 112 | PC_GRP_SHIFT_2 = 16, 113 | }__attribute__((packed)) PC_GRP_SHIFT; 114 | 115 | 116 | 117 | /* 118 | ******************************************************************************* 119 | * Global variables 120 | ******************************************************************************* 121 | */ 122 | 123 | 124 | /* 125 | ******************************************************************************* 126 | * Public functions declaration 127 | ******************************************************************************* 128 | */ 129 | 130 | int8_t PC_Init(); 131 | int8_t PC_Setup(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask, bool is_enable); 132 | void PC_QuickEnable(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask); 133 | void PC_QuickDisable(PC_GRP_IDX pc_grp_idx, uint8_t pc_pin_mask); 134 | 135 | 136 | /* 137 | ******************************************************************************* 138 | * Private functions declaration 139 | ******************************************************************************* 140 | */ 141 | 142 | 143 | /* 144 | ******************************************************************************* 145 | * Public functions 146 | ******************************************************************************* 147 | */ 148 | 149 | /** 150 | * function_example - Function example 151 | * 152 | * @param [in] input Example input. 153 | * @param [out] *p_output Example output. 154 | * 155 | * @return [int] Function executing result. 156 | * @retval [0] Success. 157 | * @retval [-1] Fail. 158 | * 159 | */ 160 | 161 | 162 | /* 163 | ******************************************************************************* 164 | * Private functions 165 | ******************************************************************************* 166 | */ 167 | 168 | #endif // PIN_CHANGE_H_ 169 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/rc_in.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file rc_in.cpp 11 | * @brief RC input pulse (PWM control signal) detector. 12 | * 13 | * @author Y.S.Kuo in Hsinchu 14 | ******************************************************************************* 15 | */ 16 | 17 | #ifndef RC_IN_H_ 18 | #define RC_IN_H_ 19 | 20 | #include "pin_change.h" 21 | 22 | 23 | /* 24 | ******************************************************************************* 25 | * Constant value definition 26 | ******************************************************************************* 27 | */ 28 | 29 | /* Logic index of RC input channels */ 30 | #define RCIN_THRO_IDX 0 31 | #define RCIN_AILE_IDX 1 32 | #define RCIN_ELEV_IDX 2 33 | #define RCIN_RUDD_IDX 3 34 | #define RCIN_AUX1_IDX 4 35 | 36 | /* Channels IO port */ 37 | #define RCIN_CH_PORT PIND 38 | 39 | /* Channel and pin change definition */ 40 | #define RCIN_CH_TOTAL 5 41 | 42 | /* Fail safe check */ 43 | #define RCIN_FAIL_COND ( \ 44 | (1 << RCIN_THRO_IDX) \ 45 | | (1 << RCIN_AILE_IDX) \ 46 | | (1 << RCIN_ELEV_IDX) \ 47 | | (1 << RCIN_RUDD_IDX) \ 48 | ) 49 | 50 | #define RCIN_PULSE_MIN_TICKS TIMER1_MICROS_TO_TICKS(900) /* 900 us */ 51 | #define RCIN_PULSE_MAX_TICKS TIMER1_MICROS_TO_TICKS(2100) /* 2100 us */ 52 | 53 | /* Middle point of RC IN signal, the ideal pulse width is 1500 us */ 54 | #define RCIN_PULSE_MIDDLE_TICKS ((RCIN_PULSE_MAX_TICKS \ 55 | + RCIN_PULSE_MIN_TICKS) / 2) 56 | 57 | #define RCIN_IS_PULSE_VALID(ticks) (ticks >= RCIN_PULSE_MIN_TICKS \ 58 | && ticks <= RCIN_PULSE_MAX_TICKS) 59 | 60 | 61 | /* 62 | ******************************************************************************* 63 | * Data type definition 64 | ******************************************************************************* 65 | */ 66 | 67 | 68 | /* 69 | ******************************************************************************* 70 | * Global variables 71 | ******************************************************************************* 72 | */ 73 | 74 | 75 | /* 76 | ******************************************************************************* 77 | * Public functions declaration 78 | ******************************************************************************* 79 | */ 80 | 81 | 82 | /* 83 | ******************************************************************************* 84 | * Private functions declaration 85 | ******************************************************************************* 86 | */ 87 | 88 | 89 | /* 90 | ******************************************************************************* 91 | * Public functions 92 | ******************************************************************************* 93 | */ 94 | 95 | int8_t RCIN_Init(); 96 | bool RCIN_FailChk(); 97 | void RCIN_SetDirection(bool *p_channel_is_reversed); 98 | void RCIN_SetNeutral(uint16_t *p_channel_neutral_ticks); 99 | void RCIN_SetMaxMinStick(uint16_t *p_max, uint16_t *p_min); 100 | void RCIN_SetFailsafe(uint16_t *p_channel_failsafe_ticks); 101 | uint8_t RCIN_ReadChannels(uint16_t *p_channels); 102 | void RCIN_GetChannelsDiff(uint16_t *p_pulse_in, int16_t *p_pulse_diff); 103 | 104 | void RCIN_PulseHandler(PC_GRP_IDX pc_grp_idx, uint32_t trig_time, 105 | uint8_t pin_status, uint8_t pin_change); 106 | 107 | 108 | /* 109 | ******************************************************************************* 110 | * Private functions 111 | ******************************************************************************* 112 | */ 113 | 114 | 115 | #endif // RC_IN_H_ 116 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/rc_out.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file rc_out.h 11 | * @brief RC output pulse (PWM control signal) generator. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef RC_OUT_H_ 17 | #define RC_OUT_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | /* Channel and pin change definition */ 27 | #define RCOUT_CH_TOTAL 4 28 | 29 | /* Logic index of RC output channels */ 30 | #define RCOUT_THRO_IDX 0 31 | #define RCOUT_AILE_IDX 1 32 | #define RCOUT_ELEV_IDX 2 33 | #define RCOUT_RUDD_IDX 3 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Data type definition 39 | ******************************************************************************* 40 | */ 41 | 42 | 43 | /* 44 | ******************************************************************************* 45 | * Global variables 46 | ******************************************************************************* 47 | */ 48 | 49 | 50 | /* 51 | ******************************************************************************* 52 | * Public functions declaration 53 | ******************************************************************************* 54 | */ 55 | 56 | 57 | /* 58 | ******************************************************************************* 59 | * Private functions declaration 60 | ******************************************************************************* 61 | */ 62 | 63 | 64 | /* 65 | ******************************************************************************* 66 | * Public functions 67 | ******************************************************************************* 68 | */ 69 | 70 | int8_t RCOUT_Init(); 71 | uint8_t RCOUT_GetCycUpdateCnt(); 72 | int8_t RCOUT_SetServoPWM(uint16_t *p_channels_width, uint8_t channel_total); 73 | 74 | 75 | /* 76 | ******************************************************************************* 77 | * Private functions 78 | ******************************************************************************* 79 | */ 80 | 81 | 82 | #endif // RC_OUT_H_ 83 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/rom_drv.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file rom_drv.cpp 11 | * 12 | * @brief AVR EEPROM driving functions. 13 | * These functions are designed for AVR 328P MCU only. 14 | * 15 | * Abbreviations: 16 | * EEPROM - Electrically-Erasable Programmable Read-Only Memory 17 | * 18 | * Hardware: 19 | * EEPROM 20 | * 21 | * Interrupts: 22 | * None 23 | * 24 | * @author Y.S.Kuo in Hsinchu 25 | ******************************************************************************* 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #include "rom_drv.h" 33 | 34 | 35 | /* 36 | ******************************************************************************* 37 | * Constant value definition 38 | ******************************************************************************* 39 | */ 40 | 41 | #if defined(__AVR_ATmega328P__) 42 | #define ROM_SPACE_SIZE 1024 43 | #else 44 | #error "Unknown MCU type" 45 | #endif 46 | 47 | 48 | /* 49 | ******************************************************************************* 50 | * Data type definition 51 | ******************************************************************************* 52 | */ 53 | 54 | 55 | /* 56 | ******************************************************************************* 57 | * Global variables 58 | ******************************************************************************* 59 | */ 60 | 61 | 62 | /* 63 | ******************************************************************************* 64 | * Public functions declaration 65 | ******************************************************************************* 66 | */ 67 | 68 | 69 | /* 70 | ******************************************************************************* 71 | * Private functions declaration 72 | ******************************************************************************* 73 | */ 74 | 75 | 76 | /* 77 | ******************************************************************************* 78 | * Public functions 79 | ******************************************************************************* 80 | */ 81 | 82 | /** 83 | * ROM_ReadBytes - Function to read ROM data. 84 | * 85 | * @param [in] rom_addr Read ROM address. 86 | * @param [in] *p_data Buffer for storing ROM data. 87 | * @param [in] data_size Byte size of output buffer. 88 | * 89 | * @return [int8_t] Function executing result. 90 | * @retval [0] Success. 91 | * @retval [-1] Fail. 92 | * 93 | */ 94 | int8_t ROM_ReadBytes(uint16_t rom_addr, uint8_t *p_data, uint16_t data_size) 95 | { 96 | if(p_data == NULL) 97 | return -1; 98 | if(data_size > ROM_SPACE_SIZE) 99 | return -1; 100 | if(rom_addr > ROM_SPACE_SIZE - data_size) /* Boundary check */ 101 | return -1; 102 | 103 | while(data_size--){ 104 | 105 | /* Make sure previous write procedure is completed */ 106 | while(EECR & _BV(EEPE)) 107 | ; 108 | 109 | /* Setup read address */ 110 | EEAR = rom_addr++; 111 | 112 | /* Start read procedure */ 113 | EECR = _BV(EERE); 114 | 115 | /* Store ROM data to buffer */ 116 | *(p_data++) = EEDR; 117 | 118 | } 119 | 120 | return 0; 121 | } 122 | 123 | /** 124 | * ROM_UpdateBytes - Function to update ROM data. 125 | * 126 | * @param [in] rom_addr Write ROM address. 127 | * @param [in] *p_data Buffer contains new data. 128 | * @param [in] data_size Byte size of input buffer. 129 | * 130 | * @return [int8_t] Function executing result. 131 | * @retval [0] Success. 132 | * @retval [-1] Fail. 133 | * 134 | */ 135 | int8_t ROM_UpdateBytes(uint16_t rom_addr, uint8_t *p_data, uint16_t data_size) 136 | { 137 | volatile uint8_t read_byte; 138 | 139 | if(p_data == NULL) 140 | return -1; 141 | if(data_size > ROM_SPACE_SIZE) 142 | return -1; 143 | if(rom_addr > ROM_SPACE_SIZE - data_size) /* Boundary check */ 144 | return -1; 145 | 146 | /* 147 | * DO NOT block the interrupt during ROM writing procedure! 148 | */ 149 | 150 | while(data_size--){ 151 | 152 | /* Make sure previous write procedure is completed */ 153 | while(EECR & _BV(EEPE)) 154 | ; 155 | 156 | /* Setup read address */ 157 | EEAR = rom_addr; 158 | 159 | /* Start read procedure */ 160 | EECR = _BV(EERE); 161 | 162 | /* Read ROM data */ 163 | read_byte = EEDR; 164 | 165 | /* Compare and update ROM data when needed */ 166 | if(read_byte != *p_data){ 167 | EEDR = *p_data; 168 | EECR = _BV(EEMPE); /* Ready to erase and write */ 169 | EECR |= _BV(EEPE); /* Start procedure */ 170 | 171 | } 172 | 173 | p_data++; 174 | rom_addr++; 175 | 176 | } 177 | 178 | return 0; 179 | } 180 | 181 | 182 | /* 183 | ******************************************************************************* 184 | * Private functions 185 | ******************************************************************************* 186 | */ 187 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/rom_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file rom_drv.h 11 | * 12 | * @brief AVR EEPROM driving functions. 13 | * These functions are designed for AVR 328P MCU only. 14 | * 15 | * @author Y.S.Kuo in Hsinchu 16 | ******************************************************************************* 17 | */ 18 | 19 | #ifndef ROM_DRV_H_ 20 | #define ROM_DRV_H_ 21 | 22 | 23 | /* 24 | ******************************************************************************* 25 | * Constant value definition 26 | ******************************************************************************* 27 | */ 28 | 29 | 30 | /* 31 | ******************************************************************************* 32 | * Data type definition 33 | ******************************************************************************* 34 | */ 35 | 36 | 37 | /* 38 | ******************************************************************************* 39 | * Global variables 40 | ******************************************************************************* 41 | */ 42 | 43 | 44 | /* 45 | ******************************************************************************* 46 | * Public functions declaration 47 | ******************************************************************************* 48 | */ 49 | 50 | 51 | /* 52 | ******************************************************************************* 53 | * Private functions declaration 54 | ******************************************************************************* 55 | */ 56 | 57 | 58 | /* 59 | ******************************************************************************* 60 | * Public functions 61 | ******************************************************************************* 62 | */ 63 | 64 | int8_t ROM_ReadBytes(uint16_t rom_addr, uint8_t *p_data, uint16_t data_size); 65 | int8_t ROM_UpdateBytes(uint16_t rom_addr, uint8_t *p_data, uint16_t data_size); 66 | 67 | 68 | /* 69 | ******************************************************************************* 70 | * Private functions 71 | ******************************************************************************* 72 | */ 73 | 74 | 75 | #endif // ROM_DRV_H_ 76 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/timers_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file timers_drv.h 11 | * @brief 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef TIMERS_DRV_H_ 17 | #define TIMERS_DRV_H_ 18 | 19 | 20 | /* 21 | ******************************************************************************* 22 | * Constant value definition 23 | ******************************************************************************* 24 | */ 25 | 26 | 27 | /* 28 | ******************************************************************************* 29 | * Timer0, Pre-scaler setting, support 0, 1, 8, 64, 256, 1024, 30 | * 1024, but please do not change default setting, otherwise it 31 | * may cause unpredictable problem (Set to 8 by default). 32 | ******************************************************************************* 33 | */ 34 | #define TIMER0_PRESCALER 8 35 | 36 | /* Timer0, Frequency, 16 Mhz / 8 = 2 Mhz by default */ 37 | #define TIMER0_FREQ (F_CPU / TIMER0_PRESCALER) 38 | 39 | /* Timer0 ticks per microsecond, 2 Mhz / 1 Mhz = 2, 2 ticks */ 40 | #define TIMER0_TICKS_PER_US (TIMER0_FREQ / 1000000) 41 | 42 | /* Timer0, convert ticks to micro seconds */ 43 | #define TIMER0_TICKS_TO_MICROS(ticks) (ticks / TIMER0_TICKS_PER_US) 44 | 45 | /* Timer0, convert micro seconds to ticks */ 46 | #define TIMER0_MICROS_TO_TICKS(micros) (micros * TIMER0_TICKS_PER_US) 47 | 48 | 49 | /* 50 | ******************************************************************************* 51 | * Timer1, Pre-scaler setting, support 0, 1, 8, 64, 256, 1024, 52 | * but please do not change default setting, otherwise it may 53 | * cause unpredictable problem (Set to 8 by default). 54 | ******************************************************************************* 55 | */ 56 | #define TIMER1_PRESCALER 8 57 | 58 | /* Timer1, Frequency, 16 Mhz / 8 = 2 Mhz by default */ 59 | #define TIMER1_FREQ (F_CPU / TIMER1_PRESCALER) 60 | 61 | /* Timer1 ticks per microsecond, 2 Mhz / 1 Mhz = 2, 2 ticks */ 62 | #define TIMER1_TICKS_PER_US (TIMER1_FREQ / 1000000) 63 | 64 | /* 65 | * Timer1 ticks per millisecond, 2 Mhz / 1 Khz = 2000, 2000 ticks, 66 | * but we give 2048 for avoiding division calculation. 67 | */ 68 | #define TIMER1_TICKS_PER_MS (2048) 69 | 70 | /* Timer1, convert ticks to micro seconds */ 71 | #define TIMER1_TICKS_TO_MICROS(ticks) (ticks / TIMER1_TICKS_PER_US) 72 | 73 | /* Timer1, convert micro seconds to ticks */ 74 | #define TIMER1_MICROS_TO_TICKS(micros) (micros * TIMER1_TICKS_PER_US) 75 | 76 | /* Timer1, convert ticks to milliseconds */ 77 | #define TIMER1_TICKS_TO_MILLIS(ticks) (ticks / TIMER1_TICKS_PER_MS) 78 | 79 | /* Timer1, convert milliseconds to ticks */ 80 | #define TIMER1_MILLIS_TO_TICKS(millis) (millis * TIMER1_TICKS_PER_MS) 81 | 82 | 83 | /* 84 | ******************************************************************************* 85 | * Timer2, Pre-scaler setting, support 0, 1, 8, 32, 64, 128, 256, 86 | * 1024, but please do not change default setting, otherwise it 87 | * may cause unpredictable problem (Set to 8 by default). 88 | ******************************************************************************* 89 | */ 90 | 91 | #define TIMER2_PRESCALER 8 92 | 93 | 94 | /* 95 | ******************************************************************************* 96 | * Data type definition 97 | ******************************************************************************* 98 | */ 99 | 100 | 101 | /* 102 | ******************************************************************************* 103 | * Global variables 104 | ******************************************************************************* 105 | */ 106 | 107 | 108 | /* 109 | ******************************************************************************* 110 | * Public functions declaration 111 | ******************************************************************************* 112 | */ 113 | 114 | 115 | /* 116 | ******************************************************************************* 117 | * Private functions declaration 118 | ******************************************************************************* 119 | */ 120 | 121 | 122 | /* 123 | ******************************************************************************* 124 | * Public functions 125 | ******************************************************************************* 126 | */ 127 | 128 | int8_t Timers_Init(); 129 | 130 | uint8_t Timer0_GetTicks8(); 131 | int8_t Timer0_SetTimerCompA(uint8_t trig_time, bool is_enable_interrupt); 132 | int8_t Timer0_SetTimerCompB(uint8_t trig_time, bool is_enable_interrupt); 133 | 134 | uint16_t Timer1_GetTicks16(); 135 | uint32_t Timer1_GetTicks32(); 136 | uint32_t Timer1_GetMicros(); 137 | uint32_t Timer1_GetMillis(); 138 | void Timer1_DelayMillis(uint32_t millis); 139 | int8_t Timer1_SetInputTriggerEdge(bool is_rising_edge); 140 | int8_t Timer1_SetInputCapture(bool is_rising_edge, bool is_noise_cancel, 141 | bool is_enable_interrupt); 142 | uint16_t Timer1_ReadInputCaptureTime(); 143 | 144 | 145 | /* 146 | ******************************************************************************* 147 | * Private functions 148 | ******************************************************************************* 149 | */ 150 | 151 | #endif // TIMERS_DRV_H_ 152 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/uart_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file uart_drv.h 11 | * @brief AVR UART driver functions. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef UART_DRV_H_ 17 | #define UART_DRV_H_ 18 | 19 | #include 20 | 21 | 22 | /* 23 | ******************************************************************************* 24 | * Constant value definition 25 | ******************************************************************************* 26 | */ 27 | 28 | 29 | /* 30 | ******************************************************************************* 31 | * Data type definition 32 | ******************************************************************************* 33 | */ 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Global variables 39 | ******************************************************************************* 40 | */ 41 | 42 | 43 | /* 44 | ******************************************************************************* 45 | * Public functions declaration 46 | ******************************************************************************* 47 | */ 48 | 49 | int8_t Uart0_Init(uint32_t baud_rate); 50 | uint8_t Uart0_WriteBytes(uint8_t *p_data, uint8_t bytes); 51 | uint8_t Uart0_WriteBytesNB(uint8_t *p_data, uint8_t bytes); 52 | uint8_t Uart0_WriteByte(uint8_t data); 53 | uint8_t Uart0_WriteByteNB(uint8_t data); 54 | uint8_t Uart0_ReadBytes(uint8_t *p_data, uint8_t bytes); 55 | uint8_t Uart0_ReadByte(uint8_t *p_data); 56 | uint8_t Uart0_ReadAvailable(); 57 | 58 | 59 | /* 60 | ******************************************************************************* 61 | * Private functions 62 | ******************************************************************************* 63 | */ 64 | 65 | 66 | #endif // UART_DRV_H_ 67 | 68 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/uart_sim.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file uart_sim.cpp 11 | * @brief Hardware/software simulated UART functions 12 | * (based on timer 1 input capturing mechanism, only support baud 9600) 13 | * 14 | * @author Y.S.Kuo in Hsinchu 15 | ******************************************************************************* 16 | */ 17 | 18 | #ifndef UART_SIM_H_ 19 | #define UART_SIM_H_ 20 | 21 | #include 22 | #include 23 | 24 | #include "pin_change.h" 25 | 26 | 27 | /* 28 | ******************************************************************************* 29 | * Constant value definition 30 | ******************************************************************************* 31 | */ 32 | 33 | #define UARTS_FUNCTION_EN true 34 | 35 | 36 | /* 37 | ******************************************************************************* 38 | * Data type definition 39 | ******************************************************************************* 40 | */ 41 | 42 | 43 | /* 44 | ******************************************************************************* 45 | * Global variables 46 | ******************************************************************************* 47 | */ 48 | 49 | 50 | /* 51 | ******************************************************************************* 52 | * Public functions declaration 53 | ******************************************************************************* 54 | */ 55 | 56 | int8_t UartS_Init(uint32_t baud_rate); 57 | uint8_t UartS_ReadBytes(uint8_t *p_data, uint8_t bytes); 58 | uint8_t UartS_ReadByte(uint8_t *p_data); 59 | uint8_t UartS_ReadAvailable(); 60 | uint8_t UartS_WriteBytes(uint8_t *p_data, uint8_t bytes); 61 | uint8_t UartS_WriteBytesNB(uint8_t *p_data, uint8_t bytes); 62 | uint8_t UartS_WriteByte(uint8_t data); 63 | uint8_t UartS_WriteByteNB(uint8_t data); 64 | void UartS_RxPulseHandler(PC_GRP_IDX pc_grp_idx, uint32_t trig_time, 65 | uint8_t pin_status, uint8_t pin_change); 66 | 67 | 68 | /* 69 | ******************************************************************************* 70 | * Private functions 71 | ******************************************************************************* 72 | */ 73 | 74 | 75 | #endif // UART_SIM_H_ 76 | 77 | -------------------------------------------------------------------------------- /OneRCFW/libraries/OneRCLib/uart_stream.h: -------------------------------------------------------------------------------- 1 | /** 2 | ******************************************************************************* 3 | * ______ _ __ ______ ____ ______ ___ ______ ____ 4 | * / __ / / \ / / / ____/ / __ \ / ___/ / / /_ _/ / __ \ 5 | * / /_/ / / \ / / ____/ / -- / / /__ __ / /__ _/ /_ / __ < 6 | * /_____/ /_/ \_/ /_____/ /__/ \_\ /_____/ /_/ /_____/ /_____/ /_____/ 7 | * 8 | * An amateur remote control software library. Use at your own risk. 9 | * 10 | * @file uart_stream.cpp 11 | * @brief AVR UART stream functions. 12 | * @author Y.S.Kuo in Hsinchu 13 | ******************************************************************************* 14 | */ 15 | 16 | #ifndef UART_STREAM_H_ 17 | #define UART_STREAM_H_ 18 | 19 | #include 20 | #include 21 | 22 | #include "uart_drv.h" 23 | 24 | 25 | /* 26 | ******************************************************************************* 27 | * Constant value definition 28 | ******************************************************************************* 29 | */ 30 | 31 | 32 | /* 33 | ******************************************************************************* 34 | * Data type definition 35 | ******************************************************************************* 36 | */ 37 | 38 | 39 | /* 40 | ******************************************************************************* 41 | * Global variables 42 | ******************************************************************************* 43 | */ 44 | 45 | 46 | /* 47 | ******************************************************************************* 48 | * Public functions declaration 49 | ******************************************************************************* 50 | */ 51 | 52 | void Uart0_PrintChr(char chr); 53 | void Uart0_PrintStr(const char *p_str); 54 | void Uart0_PrintUnsigned(uint32_t value, uint8_t base); 55 | void Uart0_PrintSigned(int32_t value); 56 | void Uart0_PrintFloat(float value, uint8_t digits); 57 | void Uart0_Printf(const char *p_fmt, ...); 58 | void Uart0_Println(const char *p_fmt, ...); 59 | 60 | void UartS_PrintChr(char chr); 61 | void UartS_PrintStr(const char *p_str); 62 | void UartS_PrintUnsigned(uint32_t value, uint8_t base); 63 | void UartS_PrintSigned(int32_t value); 64 | void UartS_PrintFloat(float value, uint8_t digits); 65 | void UartS_Printf(const char *p_fmt, ...); 66 | void UartS_Println(const char *p_fmt, ...); 67 | 68 | 69 | /* 70 | ******************************************************************************* 71 | * Private functions 72 | ******************************************************************************* 73 | */ 74 | 75 | 76 | #endif // UART_STREAM_H_ 77 | 78 | -------------------------------------------------------------------------------- /OneRCFlightGearConf/.fgfsrc: -------------------------------------------------------------------------------- 1 | --geometry=640x480 2 | --generic=socket,out,200,127.0.0.1,5501,udp,MAVLink 3 | --generic=socket,in,50,127.0.0.1,5500,udp,MAVLink 4 | --nmea=socket,out,5,127.0.0.1,5505,udp 5 | --vc=30 6 | --httpd=8080 7 | #--altitude=10000 8 | #--heading=90 9 | #--roll=0 10 | #--pitch=0 11 | #--wind=0@0 12 | #--turbulence=0.0 13 | --prop:/sim/frame-rate-throttle-hz=60 14 | --timeofday=noon 15 | --shading-flat 16 | --fog-disable 17 | --disable-specular-highlight 18 | --disable-random-objects 19 | --disable-panel 20 | --disable-horizon-effect 21 | --disable-clouds 22 | --disable-anti-alias-hud 23 | --enable-fuel-freeze 24 | 25 | --enable-terrasync 26 | 27 | --units-meters 28 | 29 | 30 | --airport=rctp 31 | ##--offset-distance=4000 32 | #--offset-azimuth=90 33 | --altitude=0 34 | --heading=0 35 | --model-hz=60 36 | --disable-random-objects 37 | --prop:/sim/rendering/texture-compression=off 38 | --prop:/sim/rendering/quality-level=0 39 | --prop:/sim/rendering/shaders/quality-level=0 40 | --disable-ai-traffic 41 | --prop:/sim/ai/enabled=0 42 | --aircraft=DR400-dauphin 43 | #--disable-sound 44 | --prop:/sim/rendering/random-vegetation=0 45 | --prop:/sim/rendering/random-buildings=0 46 | --disable-specular-highlight 47 | --disable-ai-models 48 | --disable-clouds 49 | --disable-clouds3d 50 | #--disable-textures 51 | --fog-fastest 52 | --visibility=5000 53 | --disable-distance-attenuation 54 | --disable-enhanced-lighting 55 | --disable-real-weather-fetch 56 | --prop:/sim/rendering/particles=0 57 | --prop:/sim/rendering/multi-sample-buffers=1 58 | --prop:/sim/rendering/multi-samples=2 -------------------------------------------------------------------------------- /OneRCFlightGearConf/MAVLink.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | true 9 | 10 | 11 | 12 | throttle 13 | double 14 | /controls/engines/current-engine/throttle 15 | 16 | 17 | aileron 18 | double 19 | /controls/flight/aileron 20 | 21 | 22 | elevator 23 | double 24 | /controls/flight/elevator 25 | 26 | 27 | rudder 28 | double 29 | /controls/flight/rudder 30 | 31 | 32 | 33 | 34 | true 35 | magic,0x4c56414d 36 | 37 | 38 | roll angle 39 | double 40 | /orientation/roll-deg 41 | 42 | 43 | 44 | pitch angle 45 | double 46 | /orientation/pitch-deg 47 | 48 | 49 | 50 | yaw angle 51 | double 52 | /orientation/heading-deg 53 | 54 | 55 | 56 | 57 | latitude 58 | double 59 | /position/latitude-deg 60 | 61 | 62 | longitude 63 | double 64 | /position/longitude-deg 65 | 66 | 67 | altitude 68 | double 69 | /position/altitude-ft 70 | 71 | 72 | heading 73 | double 74 | /orientation/heading-deg 75 | 76 | 77 | 78 | 79 | speed - north 80 | double 81 | /velocities/speed-north-fps 82 | 83 | 84 | speed - east 85 | double 86 | /velocities/speed-east-fps 87 | 88 | 89 | 90 | 91 | 92 | x-accel 93 | double 94 | /accelerations/pilot/x-accel-fps_sec 95 | 96 | 97 | y-accel 98 | double 99 | /accelerations/pilot/y-accel-fps_sec 100 | 101 | 102 | z-accel 103 | double 104 | /accelerations/pilot/z-accel-fps_sec 105 | 106 | 107 | roll-rate 108 | double 109 | /orientation/roll-rate-degps 110 | 111 | 112 | pitch-rate 113 | double 114 | /orientation/pitch-rate-degps 115 | 116 | 117 | yaw-rate 118 | double 119 | /orientation/yaw-rate-degps 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /OneRCGUI/MP_CRC.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class MP_crc(object): 4 | 5 | def __init__(self, buf=None): 6 | self.crc = 0xffff 7 | if buf is not None: 8 | if isinstance(buf, str): 9 | self.accumulate_str(buf) 10 | else: 11 | self.accumulate(buf) 12 | 13 | def accumulate(self, buf): 14 | '''add in some more bytes''' 15 | accum = self.crc 16 | for b in buf: 17 | tmp = b ^ (accum & 0xff) 18 | tmp = (tmp ^ (tmp<<4)) & 0xFF 19 | accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4) 20 | self.crc = accum 21 | 22 | def accumulate_str(self, buf): 23 | '''add in some more bytes''' 24 | accum = self.crc 25 | import array 26 | bytes = array.array('B') 27 | bytes.fromstring(buf) 28 | self.accumulate(bytes) -------------------------------------------------------------------------------- /OneRCGUI/MP_decoder.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | 4 | from MP_frames import * 5 | from struct import * 6 | from collections import namedtuple 7 | import numpy as np 8 | import os 9 | 10 | class MP_decoder(): 11 | 12 | __mp_frame_payload_format = '' 13 | __mp_frame_payload_fields = 'payload' 14 | __mp_frame_payload_struct = namedtuple('frame_payload_struct', __mp_frame_payload_fields) 15 | 16 | 17 | """ Decode MP frame raw bytes and convert to completed frame structure 18 | 19 | """ 20 | def __init__(self): 21 | pass 22 | 23 | 24 | """ Decode MP frame raw bytes and convert to completed frame structure 25 | 26 | """ 27 | def decode(self, raw_bytes): 28 | 29 | byte_start = 0 30 | byte_end = 0 31 | 32 | # Check input data size, should equal or bigger than size(header) + size(tail) 33 | if(len(raw_bytes) < int(MP_FRM_HDR_STRUCT[1]) + int(MP_FRM_TAIL_STRUCT[1])): 34 | return None 35 | 36 | # Decode frame header part 37 | byte_start = 0 38 | byte_end = byte_start + int(MP_FRM_HDR_STRUCT[1]) 39 | hdr = self.decode_hdr(raw_bytes[byte_start:byte_end]) 40 | 41 | # Check "len" field, and check whether it is correct or not. 42 | if(hdr.len != len(raw_bytes) - int(MP_FRM_HDR_STRUCT[1]) - int(MP_FRM_TAIL_STRUCT[1])): 43 | return None 44 | 45 | try: 46 | payload_struct = MP_FRM_TABLES[hdr.cmd] 47 | except: 48 | return None 49 | 50 | if(int(payload_struct[1]) != hdr.len): 51 | return None 52 | 53 | mp_frame_format = '=' + MP_FRM_HDR_STRUCT[2] + payload_struct[2] + MP_FRM_TAIL_STRUCT[2] 54 | mp_frame_fields = MP_FRM_HDR_STRUCT[3] + ', ' + payload_struct[3] + ', ' + MP_FRM_TAIL_STRUCT[3] 55 | mp_frame_struct = namedtuple('MP_FRM', mp_frame_fields) 56 | 57 | return mp_frame_struct._make(unpack(mp_frame_format, raw_bytes)) 58 | 59 | 60 | """ Decode MP frame raw bytes header part 61 | 62 | """ 63 | def decode_hdr(self, raw_bytes): 64 | 65 | hdr_struct = namedtuple(MP_FRM_HDR_STRUCT[0], MP_FRM_HDR_STRUCT[3]) 66 | return hdr_struct._make(unpack(MP_FRM_HDR_STRUCT[2], raw_bytes)) 67 | 68 | pass 69 | 70 | 71 | """ Decode MP frame raw bytes payload part 72 | 73 | """ 74 | def decode_payload(self, raw_bytes, byte_size): 75 | pass 76 | 77 | 78 | """ Decode MP frame raw bytes tail part 79 | 80 | """ 81 | def decode_tail(self, raw_bytes): 82 | 83 | hdr_struct = namedtuple(MP_FRM_HDR_STRUCT[0], MP_FRM_TAIL_STRUCT[3]) 84 | return hdr_struct._make(unpack(MP_FRM_HDR_STRUCT[2], raw_bytes)) 85 | 86 | pass 87 | 88 | #mp_deocder = MP_decoder() 89 | 90 | #a = mp_deocder.decode(b'\x01\x02\x03\x03\xAA\xBB\xCC\x05') 91 | #print a 92 | #print len(a) -------------------------------------------------------------------------------- /OneRCGUI/MP_fdm_nmea.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | 4 | import serial 5 | import socket 6 | from struct import * 7 | from collections import namedtuple 8 | import numpy as np 9 | from MP_handler import MP_handler 10 | import Queue 11 | import time 12 | from MP_frames import * 13 | import threading 14 | 15 | UDP_IP = "127.0.0.1" 16 | UDP_RX_PORT = 5505 17 | 18 | rx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 19 | rx_sock.bind((UDP_IP, UDP_RX_PORT)) 20 | 21 | nmea_serial = serial.Serial("COM6", 9600, timeout = 0.01, bytesize = 8, parity = 'N', stopbits = 1) 22 | 23 | __mp_rx_frame_queue = Queue.Queue(0) 24 | 25 | 26 | def UDP_RX_Thread(): 27 | 28 | while(True): 29 | data = rx_sock.recv(1024) 30 | print data 31 | nmea_msgs = data.split(); 32 | for nmea in nmea_msgs[:]: 33 | if('$GPGGA' in nmea): 34 | nmea = nmea + "\r\n" 35 | nmea_serial.write(nmea) 36 | 37 | if('$GPRMC' in nmea): 38 | nmea = nmea + "\r\n" 39 | nmea_serial.write(nmea) 40 | pass 41 | 42 | udp_rx_thread = threading.Thread(target = UDP_RX_Thread) 43 | udp_rx_thread.start() 44 | 45 | while(True): 46 | 47 | time.sleep(1) 48 | -------------------------------------------------------------------------------- /OneRCGUI/MP_fdm_sim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | 4 | import socket 5 | from struct import * 6 | from collections import namedtuple 7 | import numpy as np 8 | from MP_handler import MP_handler 9 | import Queue 10 | import time 11 | from MP_frames import * 12 | import threading 13 | 14 | UDP_IP = "127.0.0.1" 15 | UDP_TX_PORT = 5500 16 | UDP_RX_PORT = 5501 17 | 18 | FLIGHTGEAR_RX_DEFINE = np.array( 19 | [ 20 | ['d', 'roll_angle'], # double 21 | ['d', 'pitch_angle'], # double 22 | ['d', 'yaw_angle'], # double 23 | ['d', 'latitude'], # double 24 | ['d', 'longitude'], # double 25 | ['d', 'altitude'], # double 26 | ['d', 'heading'], # double 27 | ['d', 'speed_north'], # double 28 | ['d', 'speed_east'], # double 29 | ['d', 'x_accel'], # double 30 | ['d', 'y_accel'], # double 31 | ['d', 'z_accel'], # double 32 | ['d', 'roll_rate'], # double 33 | ['d', 'pitch_rate'], # double 34 | ['d', 'yaw_rate'], # double 35 | ['I', 'magic'], # unsigned int 36 | ]) 37 | FLIGHTGEAR_RX_STRUCT = np.array( 38 | [ 39 | 'MP_HDR', # Name 40 | calcsize('!' + ''.join(FLIGHTGEAR_RX_DEFINE[:, 0])), # Size 41 | ''.join(FLIGHTGEAR_RX_DEFINE[:, 0]), # Field data type 42 | ', '.join(FLIGHTGEAR_RX_DEFINE[:, 1]), # Field name 43 | 44 | ]) 45 | 46 | FLIGHTGEAR_TX_DEFINE = np.array( 47 | [ 48 | ['d', 'throttle', 0], # double 49 | ['d', 'aileron', 0], # double 50 | ['d', 'elevator', 0], # double 51 | ['d', 'rudder', 0], # double 52 | ]) 53 | FLIGHTGEAR_TX_STRUCT = np.array( 54 | [ 55 | 'MP_HDR', # Name 56 | calcsize('!' + ''.join(FLIGHTGEAR_TX_DEFINE[:, 0])), # Size 57 | ''.join(FLIGHTGEAR_TX_DEFINE[:, 0]), # Field data type 58 | ', '.join(FLIGHTGEAR_TX_DEFINE[:, 1]), # Field name 59 | ', '.join(FLIGHTGEAR_TX_DEFINE[:, 2]), # Initial value 60 | 61 | ]) 62 | 63 | 64 | tx_frame_format = '!' + FLIGHTGEAR_TX_STRUCT[2] 65 | tx_frame_fields = FLIGHTGEAR_TX_STRUCT[3] 66 | tx_frame_struct = namedtuple('FLIGHTGEAR_TX', FLIGHTGEAR_TX_STRUCT[3]) 67 | 68 | 69 | tx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP 70 | rx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP 71 | 72 | rx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 73 | rx_sock.bind((UDP_IP, UDP_RX_PORT)) 74 | 75 | __mp_rx_frame_queue = Queue.Queue(0) 76 | __mp_handler = MP_handler() 77 | __mp_handler.set_rx_frame_queue(__mp_rx_frame_queue) 78 | __mp_handler.open_serial('COM3', int(57600)) 79 | __mp_handler.thread_start() 80 | 81 | def restrict (val, minval, maxval): 82 | if val < minval: return minval 83 | if val > maxval: return maxval 84 | return val 85 | 86 | def UDP_RX_Thread(): 87 | 88 | while(True): 89 | data = rx_sock.recv(1024) 90 | 91 | mp_frame_format = '!' + FLIGHTGEAR_RX_STRUCT[2] 92 | mp_frame_fields = FLIGHTGEAR_RX_STRUCT[3] 93 | mp_frame_struct = namedtuple('FLIGHTGEAR_RX', mp_frame_fields) 94 | 95 | flightgear_info = mp_frame_struct._make(unpack(mp_frame_format, data)) 96 | 97 | mp_x_accel16 = int(flightgear_info.x_accel / 32.2 * 4096.0) #- == pitch down 98 | mp_y_accel16 = int(flightgear_info.y_accel / 32.2 * -4096.0) #- right down 99 | mp_z_accel16 = int(flightgear_info.z_accel / 32.2 * -4096.0) #- in FG 100 | 101 | mp_x_gyro16 = int(flightgear_info.roll_rate * 16.4) #+ right 102 | mp_y_gyro16 = int(flightgear_info.pitch_rate * -16.4) #+ up 103 | mp_z_gyro16 = int(flightgear_info.yaw_rate * -16.4) #+ right 104 | 105 | mp_x_accel16 = restrict(mp_x_accel16, -32768, 32767) 106 | mp_y_accel16 = restrict(mp_y_accel16, -32768, 32767) 107 | mp_z_accel16 = restrict(mp_z_accel16, -32768, 32767) 108 | mp_x_gyro16 = restrict(mp_x_gyro16, -32768, 32767) 109 | mp_y_gyro16 = restrict(mp_y_gyro16, -32768, 32767) 110 | mp_z_gyro16 = restrict(mp_z_gyro16, -32768, 32767) 111 | """ 112 | tx_mpu_payload = pack("<" + MP_IMU_SENSOR_STRUCT[2], 113 | 0, 114 | mp_x_accel16, mp_y_accel16, mp_z_accel16, 115 | mp_x_gyro16, mp_y_gyro16, mp_z_gyro16, 116 | 0) 117 | 118 | __mp_handler.transmit_frame(MP_TX_IMU_SENSOR_DATA_ID, tx_mpu_payload) 119 | """ 120 | tx_mpu_payload = pack(" 255): 94 | self.__frame_tx_sequence = 0 95 | 96 | def __serial_transmitter(self): 97 | 98 | while(self.__is_transmitter_en): 99 | 100 | try: 101 | tx_data = self.__serial_tx_queue.get(True, 1.0) 102 | 103 | if(tx_data): 104 | output_byte = self.__serial.write(tx_data) 105 | if(output_byte != len(tx_data)): 106 | print "Uart TX data length is mismatched" 107 | 108 | except: 109 | pass 110 | pass 111 | 112 | 113 | def set_rx_frame_queue(self, rx_frame_queue): 114 | self.__frame_rx_queue = rx_frame_queue 115 | 116 | def __serial_receiver(self): 117 | 118 | while(self.__is_receiver_en): 119 | 120 | serial_input = self.__serial.read(100000) 121 | if(serial_input): 122 | self.__serial_rx_queue.put(serial_input, True, None) 123 | 124 | pass 125 | 126 | def __frame_detector(self): 127 | 128 | while(self.__is_detector_en): 129 | 130 | try: 131 | input = self.__serial_rx_queue.get(True, 1.0) 132 | 133 | for byte in input: 134 | 135 | num = ord(byte) 136 | 137 | # Start to collect frame bytes 138 | if(self.__frame_sflag == 0x7E): 139 | 140 | self.__frame_string += byte 141 | self.__frame_rx_bytes += 1 142 | 143 | # Frame command field 144 | if(self.__frame_rx_bytes == 2): 145 | self.__frame_rx_cmd = num 146 | # Frame sequence filed 147 | elif(self.__frame_rx_bytes == 3): 148 | sequence_tmp = num 149 | # Frame length field 150 | elif(self.__frame_rx_bytes == 4): 151 | self.__frame_checksum_offset = self.__frame_rx_bytes + num + 1 152 | 153 | # Compare CRC 154 | if(self.__frame_rx_bytes == self.__frame_checksum_offset + 1): 155 | 156 | self.__rx_crc_object.accumulate_str(self.__frame_string[1:-2]) 157 | 158 | # Put complete frame in the queue 159 | frame_struct = self.decode(self.__frame_string) 160 | 161 | if(frame_struct != None): 162 | 163 | # CRC pass 164 | if(frame_struct.CRC16 == self.__rx_crc_object.crc): 165 | 166 | self.__rx_frm_cnt += 1 167 | 168 | # Check frame sequence 169 | self.__frame_rx_sequence += 1 170 | if(self.__frame_rx_sequence > 255): 171 | self.__frame_rx_sequence = 0 172 | 173 | # Increase frame drop counter if sequence is mismatched 174 | if(self.__frame_rx_sequence != sequence_tmp): 175 | self.__rx_seq_err_cnt += 1 176 | self.__frame_drop += 1 177 | self.__frame_rx_sequence = sequence_tmp 178 | 179 | if(self.__frame_rx_queue != None): 180 | 181 | rx_time = datetime.datetime.now().strftime("%H:%M:%S.%f") 182 | rx_frame = {"data" : frame_struct, "rx_time" : rx_time} 183 | 184 | self.__frame_rx_queue.put(rx_frame, True, None) 185 | else: 186 | print frame_struct 187 | 188 | # Incorrect CRC 189 | else: 190 | self.__rx_crc_err_cnt += 1 191 | print "Frm cnt, CRC err, seq err = ", self.__rx_frm_cnt, ", ", self.__rx_crc_err_cnt, ", ", self.__rx_seq_err_cnt 192 | 193 | else: 194 | self.__rx_crc_err_cnt += 1 195 | print "Frm cnt, CRC err, seq err = ", self.__rx_frm_cnt, ", ", self.__rx_crc_err_cnt, ", ", self.__rx_seq_err_cnt 196 | 197 | 198 | # Reset RX frame status 199 | self.__frame_sflag = 0x00 200 | 201 | # Detect frame start flag 202 | elif(num == 0x7E): 203 | self.__frame_sflag = 0x7E 204 | self.__frame_rx_bytes = 1 205 | self.__frame_checksum_offset = 0 206 | 207 | self.__frame_string = byte 208 | 209 | self.__rx_crc_object.crc = 0xFFFF 210 | except: 211 | pass 212 | 213 | def thread_start(self): 214 | 215 | self.__frame_sflag = 0 216 | self.__is_transmitter_en = True 217 | self.__is_receiver_en = True 218 | self.__is_detector_en = True 219 | self.__serial_rx_queue = Queue.Queue(0) 220 | 221 | self.__transmitter_thread = threading.Thread(target = self.__serial_transmitter) 222 | self.__receiver_thread = threading.Thread(target = self.__serial_receiver) 223 | self.__detector_thread = threading.Thread(target = self.__frame_detector) 224 | self.__transmitter_thread.start() 225 | self.__receiver_thread.start() 226 | self.__detector_thread.start() 227 | 228 | pass 229 | 230 | def thread_stop(self): 231 | 232 | self.__is_transmitter_en = False 233 | self.__is_receiver_en = False 234 | self.__is_detector_en = False 235 | 236 | self.__transmitter_thread.join() 237 | self.__receiver_thread.join() 238 | self.__detector_thread.join() 239 | 240 | pass 241 | 242 | """ 243 | mp_handler = MP_handler() 244 | mp_handler.open_serial("COM6", 115200) 245 | mp_handler.thread_start() 246 | """ 247 | 248 | -------------------------------------------------------------------------------- /OneRCGUI/MP_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # -*- coding: UTF-8 -*- 3 | 4 | import wx 5 | 6 | # The recommended way to use wx with mpl is with the WXAgg 7 | # backend. 8 | # 9 | import matplotlib 10 | matplotlib.use('WXAgg') 11 | import matplotlib.pyplot as plt 12 | from matplotlib.figure import Figure 13 | from matplotlib.backends.backend_wxagg import \ 14 | FigureCanvasWxAgg as FigCanvas, \ 15 | NavigationToolbar2WxAgg as NavigationToolbar 16 | 17 | import numpy as np 18 | import pylab 19 | 20 | class MP_Plot(): 21 | 22 | __plot_title = '' 23 | __plot_total = int(0) 24 | __plot_Y_range = int(180) 25 | __plot_auto_relim = False 26 | __canvas_dpi = 800 27 | __plot_data = None 28 | __data_limit_size = 1000 29 | __color = 'r' 30 | 31 | def __init__(self, panel, title = 'New plot', auto_relim = False, color = 'r'): 32 | 33 | self.public_data = [] 34 | self.paused = False 35 | self.__plot_title = title 36 | self.__plot_auto_relim = auto_relim 37 | self.__color = color 38 | 39 | self.create_main_panel(panel) 40 | 41 | def create_main_panel(self, panel): 42 | self.panel = panel 43 | 44 | self.init_plot() 45 | self.canvas = FigCanvas(self.panel, -1, self.fig) 46 | 47 | self.vbox = wx.BoxSizer(wx.VERTICAL) 48 | self.vbox.Add(self.canvas, 1, flag=wx.LEFT | wx.TOP | wx.GROW) 49 | 50 | self.panel.SetSizer(self.vbox) 51 | self.panel.Layout() 52 | self.canvas.draw() 53 | 54 | def init_plot(self): 55 | 56 | self.fig = plt.figure(figsize=plt.figaspect(0.5)) 57 | 58 | self.axes = self.fig.add_subplot(111) 59 | self.axes.set_axis_bgcolor('white') 60 | self.axes.set_title(self.__plot_title, size=8) 61 | self.axes.grid(True, color='gray') 62 | 63 | self.axes.set_ybound(-self.__plot_Y_range, self.__plot_Y_range) 64 | 65 | pylab.setp(self.axes.get_xticklabels(), fontsize=8) 66 | pylab.setp(self.axes.get_yticklabels(), fontsize=8) 67 | 68 | self.__plot_data, = self.axes.plot([], [], color = self.__color, linewidth = 1) 69 | 70 | def draw_plot(self): 71 | """ Redraws the plot 72 | """ 73 | 74 | if(self.paused == True): 75 | return 76 | 77 | plot_data = [] 78 | plot_data.extend(self.public_data) 79 | 80 | if(len(plot_data) == 0): 81 | return 82 | 83 | x_data = np.arange(len(plot_data)) + self.__plot_total 84 | y_data = np.array(plot_data) 85 | 86 | self.__plot_data.set_xdata(x_data) 87 | self.__plot_data.set_ydata(y_data) 88 | 89 | if(self.__plot_auto_relim == True): 90 | self.axes.set_ybound(round(max(y_data) + 0.5), round(min(y_data) - 0.5)) 91 | else: 92 | self.axes.set_ybound(-self.__plot_Y_range, self.__plot_Y_range) 93 | self.axes.set_xbound(round(max(x_data) + 0.5), round(min(x_data) - 0.5)) 94 | 95 | self.panel.Layout() 96 | self.canvas.draw() 97 | 98 | def add_data(self, data): 99 | if(len(self.public_data) >= self.__data_limit_size): 100 | self.public_data.pop(0) 101 | 102 | self.public_data.append(data) 103 | 104 | self.__plot_total += 1 105 | 106 | def set_y_range(self, y_new): 107 | try: 108 | y_tmp = int(y_new) 109 | self.__plot_Y_range = y_tmp 110 | return True 111 | except ValueError: 112 | return False 113 | except Exception: 114 | return False 115 | 116 | def get_y_range(self): 117 | return self.__plot_Y_range 118 | 119 | def set_auto_relim(self, enable = True): 120 | self.__plot_auto_relim = enable 121 | 122 | def set_pause(self, is_set = True): 123 | self.paused = is_set 124 | 125 | def reset(self): 126 | self.public_data = [] 127 | self.__plot_total = 0 128 | 129 | def save_plot(self, path): 130 | self.canvas.print_figure(path, dpi=self.__canvas_dpi) 131 | self.panel.Layout() 132 | self.canvas.draw() 133 | 134 | def on_exit(self, event): 135 | self.Destroy() 136 | -------------------------------------------------------------------------------- /OneRCGUI/install.txt: -------------------------------------------------------------------------------- 1 | python -m pip install numpy 2 | python -m pip install matplotlib 3 | python -m pip install PyOpenGL 4 | python -m pip install pysdl2 -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo.bmp -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo.pptx -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo_8bits.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCLibLogo_8bits.bmp -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.docx -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.pdf -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.png -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8.xlsx -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_BOM.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_BOM.pdf -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GBO: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | M02* 11 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GBS: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | %ADD10C,0.08200*% 11 | %ADD11C,0.05556*% 12 | %ADD12C,0.05359*% 13 | %ADD13C,0.04769*% 14 | %ADD14C,0.03800*% 15 | %ADD15C,0.04400*% 16 | %ADD16C,0.03900*% 17 | D10* 18 | X0065102Y0048102D03* 19 | X0075102Y0048102D03* 20 | X0085102Y0048102D03* 21 | X0085102Y0058102D03* 22 | X0075102Y0058102D03* 23 | X0065102Y0058102D03* 24 | X0065102Y0068102D03* 25 | X0075102Y0068102D03* 26 | X0085102Y0068102D03* 27 | X0085102Y0078102D03* 28 | X0075102Y0078102D03* 29 | X0065102Y0078102D03* 30 | X0065102Y0088102D03* 31 | X0075102Y0088102D03* 32 | X0085102Y0088102D03* 33 | X0085102Y0098102D03* 34 | X0075102Y0098102D03* 35 | X0065102Y0098102D03* 36 | X0065102Y0108102D03* 37 | X0075102Y0108102D03* 38 | X0085102Y0108102D03* 39 | X0085102Y0118102D03* 40 | X0075102Y0118102D03* 41 | X0065102Y0118102D03* 42 | X0065102Y0128102D03* 43 | X0075102Y0128102D03* 44 | X0085102Y0128102D03* 45 | X0085102Y0138102D03* 46 | X0075102Y0138102D03* 47 | X0065102Y0138102D03* 48 | D11* 49 | X0166102Y0137102D03* 50 | X0171102Y0137102D03* 51 | X0176102Y0137102D03* 52 | X0181102Y0137102D03* 53 | X0214102Y0126102D03* 54 | X0214102Y0121102D03* 55 | X0214102Y0116102D03* 56 | X0214102Y0111102D03* 57 | X0214102Y0106102D03* 58 | X0214102Y0101102D03* 59 | X0214102Y0096102D03* 60 | X0199102Y0071102D03* 61 | X0194102Y0071102D03* 62 | X0189102Y0071102D03* 63 | D12* 64 | X0149102Y0056102D03* 65 | X0144102Y0056102D03* 66 | X0144102Y0051102D03* 67 | X0149102Y0051102D03* 68 | X0149102Y0046102D03* 69 | X0144102Y0046102D03* 70 | D13* 71 | X0120484Y0047890D02* 72 | X0120484Y0051858D01* 73 | X0115563Y0051858D02* 74 | X0115563Y0047890D01* 75 | X0110642Y0047890D02* 76 | X0110642Y0051858D01* 77 | X0105720Y0051858D02* 78 | X0105720Y0047890D01* 79 | X0127799Y0134346D02* 80 | X0127799Y0138315D01* 81 | X0132720Y0138315D02* 82 | X0132720Y0134346D01* 83 | X0137642Y0134346D02* 84 | X0137642Y0138315D01* 85 | X0142563Y0138315D02* 86 | X0142563Y0134346D01* 87 | X0147484Y0134346D02* 88 | X0147484Y0138315D01* 89 | X0152406Y0138315D02* 90 | X0152406Y0134346D01* 91 | D14* 92 | X0144102Y0121102D03* 93 | X0133102Y0117102D03* 94 | X0131102Y0110102D03* 95 | X0125102Y0110102D03* 96 | X0110102Y0110102D03* 97 | X0109102Y0102102D03* 98 | X0109102Y0095102D03* 99 | X0109102Y0088102D03* 100 | X0094102Y0088102D03* 101 | X0094102Y0095102D03* 102 | X0094102Y0102102D03* 103 | X0094102Y0109102D03* 104 | X0094102Y0116102D03* 105 | X0117102Y0121102D03* 106 | X0151102Y0108102D03* 107 | X0163102Y0102102D03* 108 | X0157102Y0099102D03* 109 | X0157102Y0095102D03* 110 | X0145202Y0090602D03* 111 | X0141702Y0094102D03* 112 | X0131102Y0093102D03* 113 | X0137102Y0084102D03* 114 | X0126102Y0082102D03* 115 | X0132102Y0075102D03* 116 | X0136102Y0076102D03* 117 | X0149102Y0070102D03* 118 | X0154302Y0068902D03* 119 | X0163102Y0070102D03* 120 | X0156102Y0080102D03* 121 | X0163102Y0086102D03* 122 | X0183102Y0095102D03* 123 | X0187102Y0091102D03* 124 | X0204102Y0076102D03* 125 | X0185802Y0067502D03* 126 | X0170402Y0059802D03* 127 | X0163102Y0054102D03* 128 | X0138102Y0062102D03* 129 | X0132102Y0063102D03* 130 | X0134102Y0057102D03* 131 | X0119102Y0060102D03* 132 | X0116102Y0063102D03* 133 | X0110102Y0069102D03* 134 | X0115102Y0057102D03* 135 | X0194102Y0044102D03* 136 | X0205102Y0110102D03* 137 | X0191328Y0114328D03* 138 | X0186102Y0111102D03* 139 | X0178102Y0117102D03* 140 | X0169102Y0118102D03* 141 | X0184102Y0132102D03* 142 | X0201102Y0129102D03* 143 | D15* 144 | X0121102Y0137102D03* 145 | D16* 146 | X0144102Y0097102D03* 147 | M02* 148 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GKO: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | %ADD10C,0.00000*% 11 | D10* 12 | X0059102Y0037102D02* 13 | X0059102Y0149102D01* 14 | X0223102Y0149102D01* 15 | X0223102Y0037102D01* 16 | X0059102Y0037102D01* 17 | M02* 18 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GML: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | M02* 11 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GTP: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | %ADD10R,0.03346X0.03543*% 11 | %ADD11R,0.03543X0.03346*% 12 | %ADD12R,0.06299X0.05512*% 13 | %ADD13C,0.01181*% 14 | %ADD14R,0.05512X0.06299*% 15 | %ADD15R,0.05118X0.03543*% 16 | %ADD16R,0.03000X0.05000*% 17 | %ADD17R,0.02362X0.02362*% 18 | %ADD18R,0.05000X0.02200*% 19 | %ADD19R,0.02200X0.05000*% 20 | %ADD20R,0.02165X0.04724*% 21 | %ADD21R,0.03346X0.01496*% 22 | %ADD22R,0.01496X0.03346*% 23 | %ADD23C,0.01772*% 24 | %ADD24R,0.01969X0.02362*% 25 | D10* 26 | X0099819Y0081102D03* 27 | X0104386Y0081102D03* 28 | X0104386Y0088102D03* 29 | X0099819Y0088102D03* 30 | X0099819Y0095102D03* 31 | X0104386Y0095102D03* 32 | X0104386Y0102102D03* 33 | X0099819Y0102102D03* 34 | X0099819Y0109102D03* 35 | X0104386Y0109102D03* 36 | X0104386Y0116102D03* 37 | X0099819Y0116102D03* 38 | X0124819Y0121102D03* 39 | X0129386Y0121102D03* 40 | X0129386Y0127102D03* 41 | X0124819Y0127102D03* 42 | X0137819Y0127102D03* 43 | X0142386Y0127102D03* 44 | X0149819Y0121102D03* 45 | X0154386Y0121102D03* 46 | X0198819Y0121102D03* 47 | X0203386Y0121102D03* 48 | D11* 49 | X0190102Y0122819D03* 50 | X0184102Y0122819D03* 51 | X0184102Y0127386D03* 52 | X0190102Y0127386D03* 53 | X0190102Y0135819D03* 54 | X0190102Y0140386D03* 55 | X0178102Y0127386D03* 56 | X0172102Y0127386D03* 57 | X0166102Y0127386D03* 58 | X0166102Y0122819D03* 59 | X0172102Y0122819D03* 60 | X0178102Y0122819D03* 61 | X0212102Y0135819D03* 62 | X0212102Y0140386D03* 63 | X0217102Y0140386D03* 64 | X0217102Y0135819D03* 65 | X0158102Y0089386D03* 66 | X0158102Y0084819D03* 67 | X0187102Y0085386D03* 68 | X0192102Y0085386D03* 69 | X0197102Y0085386D03* 70 | X0203102Y0085386D03* 71 | X0209102Y0085386D03* 72 | X0209102Y0080819D03* 73 | X0203102Y0080819D03* 74 | X0197102Y0080819D03* 75 | X0192102Y0080819D03* 76 | X0187102Y0080819D03* 77 | X0189102Y0062386D03* 78 | X0189102Y0057819D03* 79 | X0199102Y0057819D03* 80 | X0199102Y0062386D03* 81 | X0215102Y0080819D03* 82 | X0215102Y0085386D03* 83 | X0132102Y0050386D03* 84 | X0132102Y0045819D03* 85 | D12* 86 | X0112102Y0127591D03* 87 | X0101102Y0127591D03* 88 | X0101102Y0138614D03* 89 | X0112102Y0138614D03* 90 | D13* 91 | X0187724Y0052933D02* 92 | X0190480Y0052933D01* 93 | X0190480Y0050177D01* 94 | X0187724Y0050177D01* 95 | X0187724Y0052933D01* 96 | X0187724Y0051357D02* 97 | X0190480Y0051357D01* 98 | X0190480Y0052537D02* 99 | X0187724Y0052537D01* 100 | X0187724Y0046028D02* 101 | X0190480Y0046028D01* 102 | X0190480Y0043272D01* 103 | X0187724Y0043272D01* 104 | X0187724Y0046028D01* 105 | X0187724Y0044452D02* 106 | X0190480Y0044452D01* 107 | X0190480Y0045632D02* 108 | X0187724Y0045632D01* 109 | X0197724Y0046028D02* 110 | X0200480Y0046028D01* 111 | X0200480Y0043272D01* 112 | X0197724Y0043272D01* 113 | X0197724Y0046028D01* 114 | X0197724Y0044452D02* 115 | X0200480Y0044452D01* 116 | X0200480Y0045632D02* 117 | X0197724Y0045632D01* 118 | X0197724Y0052933D02* 119 | X0200480Y0052933D01* 120 | X0200480Y0050177D01* 121 | X0197724Y0050177D01* 122 | X0197724Y0052933D01* 123 | X0197724Y0051357D02* 124 | X0200480Y0051357D01* 125 | X0200480Y0052537D02* 126 | X0197724Y0052537D01* 127 | D14* 128 | X0164803Y0048102D03* 129 | X0164803Y0064102D03* 130 | X0164803Y0080102D03* 131 | X0164803Y0096102D03* 132 | D15* 133 | X0177992Y0092362D03* 134 | X0177992Y0099843D03* 135 | X0177992Y0083843D03* 136 | X0177992Y0076362D03* 137 | X0177992Y0067843D03* 138 | X0177992Y0060362D03* 139 | X0177992Y0051843D03* 140 | X0177992Y0044362D03* 141 | D16* 142 | X0209602Y0044535D03* 143 | X0214602Y0044535D03* 144 | X0214602Y0073669D03* 145 | X0209602Y0073669D03* 146 | D17* 147 | X0174661Y0107461D03* 148 | X0169543Y0107461D03* 149 | X0162661Y0107461D03* 150 | X0157543Y0107461D03* 151 | X0160102Y0114744D03* 152 | X0172102Y0114744D03* 153 | D18* 154 | X0150002Y0097126D03* 155 | X0150002Y0093976D03* 156 | X0150002Y0090827D03* 157 | X0150002Y0087677D03* 158 | X0150002Y0084528D03* 159 | X0150002Y0081378D03* 160 | X0150002Y0078228D03* 161 | X0150002Y0075079D03* 162 | X0116202Y0075079D03* 163 | X0116202Y0078228D03* 164 | X0116202Y0081378D03* 165 | X0116202Y0084528D03* 166 | X0116202Y0087677D03* 167 | X0116202Y0090827D03* 168 | X0116202Y0093976D03* 169 | X0116202Y0097126D03* 170 | D19* 171 | X0122079Y0103002D03* 172 | X0125228Y0103002D03* 173 | X0128378Y0103002D03* 174 | X0131528Y0103002D03* 175 | X0134677Y0103002D03* 176 | X0137827Y0103002D03* 177 | X0140976Y0103002D03* 178 | X0144126Y0103002D03* 179 | X0144126Y0069202D03* 180 | X0140976Y0069202D03* 181 | X0137827Y0069202D03* 182 | X0134677Y0069202D03* 183 | X0131528Y0069202D03* 184 | X0128378Y0069202D03* 185 | X0125228Y0069202D03* 186 | X0122079Y0069202D03* 187 | D20* 188 | X0197362Y0129984D03* 189 | X0204843Y0129984D03* 190 | X0204843Y0140221D03* 191 | X0201102Y0140221D03* 192 | X0197362Y0140221D03* 193 | D21* 194 | X0202878Y0107024D03* 195 | X0202878Y0105055D03* 196 | X0202878Y0103087D03* 197 | X0202878Y0101118D03* 198 | X0202878Y0099150D03* 199 | X0202878Y0097181D03* 200 | X0187327Y0097181D03* 201 | X0187327Y0099150D03* 202 | X0187327Y0101118D03* 203 | X0187327Y0103087D03* 204 | X0187327Y0105055D03* 205 | X0187327Y0107024D03* 206 | D22* 207 | X0190181Y0109878D03* 208 | X0192150Y0109878D03* 209 | X0194118Y0109878D03* 210 | X0196087Y0109878D03* 211 | X0198055Y0109878D03* 212 | X0200024Y0109878D03* 213 | X0200024Y0094327D03* 214 | X0198055Y0094327D03* 215 | X0196087Y0094327D03* 216 | X0194118Y0094327D03* 217 | X0192150Y0094327D03* 218 | X0190181Y0094327D03* 219 | D23* 220 | X0104966Y0064378D02* 221 | X0099238Y0064378D01* 222 | X0099238Y0069102D02* 223 | X0104966Y0069102D01* 224 | X0104966Y0073827D02* 225 | X0099238Y0073827D01* 226 | D24* 227 | X0150134Y0127102D03* 228 | X0154071Y0127102D03* 229 | M02* 230 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.GTS: -------------------------------------------------------------------------------- 1 | G75* 2 | %MOIN*% 3 | %OFA0B0*% 4 | %FSLAX25Y25*% 5 | %IPPOS*% 6 | %LPD*% 7 | %AMOC8* 8 | 5,1,8,0,0,1.08239X$1,22.5* 9 | % 10 | %ADD10R,0.04146X0.04343*% 11 | %ADD11R,0.04343X0.04146*% 12 | %ADD12R,0.07099X0.06312*% 13 | %ADD13C,0.01421*% 14 | %ADD14C,0.08200*% 15 | %ADD15C,0.05556*% 16 | %ADD16R,0.06312X0.07099*% 17 | %ADD17R,0.05918X0.04343*% 18 | %ADD18R,0.03800X0.05800*% 19 | %ADD19R,0.03162X0.03162*% 20 | %ADD20R,0.05800X0.03000*% 21 | %ADD21R,0.03000X0.05800*% 22 | %ADD22R,0.02965X0.05524*% 23 | %ADD23R,0.03346X0.01496*% 24 | %ADD24R,0.01496X0.03346*% 25 | %ADD25C,0.05359*% 26 | %ADD26C,0.04769*% 27 | %ADD27C,0.02572*% 28 | %ADD28R,0.06040X0.13760*% 29 | %ADD29R,0.02769X0.03162*% 30 | %ADD30C,0.03800*% 31 | %ADD31C,0.04400*% 32 | %ADD32C,0.03900*% 33 | D10* 34 | X0099819Y0081102D03* 35 | X0104386Y0081102D03* 36 | X0104386Y0088102D03* 37 | X0099819Y0088102D03* 38 | X0099819Y0095102D03* 39 | X0104386Y0095102D03* 40 | X0104386Y0102102D03* 41 | X0099819Y0102102D03* 42 | X0099819Y0109102D03* 43 | X0104386Y0109102D03* 44 | X0104386Y0116102D03* 45 | X0099819Y0116102D03* 46 | X0124819Y0121102D03* 47 | X0129386Y0121102D03* 48 | X0129386Y0127102D03* 49 | X0124819Y0127102D03* 50 | X0137819Y0127102D03* 51 | X0142386Y0127102D03* 52 | X0149819Y0121102D03* 53 | X0154386Y0121102D03* 54 | X0198819Y0121102D03* 55 | X0203386Y0121102D03* 56 | D11* 57 | X0190102Y0122819D03* 58 | X0184102Y0122819D03* 59 | X0184102Y0127386D03* 60 | X0190102Y0127386D03* 61 | X0190102Y0135819D03* 62 | X0190102Y0140386D03* 63 | X0178102Y0127386D03* 64 | X0172102Y0127386D03* 65 | X0166102Y0127386D03* 66 | X0166102Y0122819D03* 67 | X0172102Y0122819D03* 68 | X0178102Y0122819D03* 69 | X0212102Y0135819D03* 70 | X0212102Y0140386D03* 71 | X0217102Y0140386D03* 72 | X0217102Y0135819D03* 73 | X0158102Y0089386D03* 74 | X0158102Y0084819D03* 75 | X0187102Y0085386D03* 76 | X0192102Y0085386D03* 77 | X0197102Y0085386D03* 78 | X0203102Y0085386D03* 79 | X0209102Y0085386D03* 80 | X0209102Y0080819D03* 81 | X0203102Y0080819D03* 82 | X0197102Y0080819D03* 83 | X0192102Y0080819D03* 84 | X0187102Y0080819D03* 85 | X0189102Y0062386D03* 86 | X0189102Y0057819D03* 87 | X0199102Y0057819D03* 88 | X0199102Y0062386D03* 89 | X0215102Y0080819D03* 90 | X0215102Y0085386D03* 91 | X0132102Y0050386D03* 92 | X0132102Y0045819D03* 93 | D12* 94 | X0112102Y0127591D03* 95 | X0101102Y0127591D03* 96 | X0101102Y0138614D03* 97 | X0112102Y0138614D03* 98 | D13* 99 | X0187444Y0053213D02* 100 | X0190760Y0053213D01* 101 | X0190760Y0049897D01* 102 | X0187444Y0049897D01* 103 | X0187444Y0053213D01* 104 | X0187444Y0051317D02* 105 | X0190760Y0051317D01* 106 | X0190760Y0052737D02* 107 | X0187444Y0052737D01* 108 | X0187444Y0046308D02* 109 | X0190760Y0046308D01* 110 | X0190760Y0042992D01* 111 | X0187444Y0042992D01* 112 | X0187444Y0046308D01* 113 | X0187444Y0044412D02* 114 | X0190760Y0044412D01* 115 | X0190760Y0045832D02* 116 | X0187444Y0045832D01* 117 | X0197444Y0046308D02* 118 | X0200760Y0046308D01* 119 | X0200760Y0042992D01* 120 | X0197444Y0042992D01* 121 | X0197444Y0046308D01* 122 | X0197444Y0044412D02* 123 | X0200760Y0044412D01* 124 | X0200760Y0045832D02* 125 | X0197444Y0045832D01* 126 | X0197444Y0053213D02* 127 | X0200760Y0053213D01* 128 | X0200760Y0049897D01* 129 | X0197444Y0049897D01* 130 | X0197444Y0053213D01* 131 | X0197444Y0051317D02* 132 | X0200760Y0051317D01* 133 | X0200760Y0052737D02* 134 | X0197444Y0052737D01* 135 | D14* 136 | X0065102Y0048102D03* 137 | X0075102Y0048102D03* 138 | X0085102Y0048102D03* 139 | X0085102Y0058102D03* 140 | X0075102Y0058102D03* 141 | X0065102Y0058102D03* 142 | X0065102Y0068102D03* 143 | X0075102Y0068102D03* 144 | X0085102Y0068102D03* 145 | X0085102Y0078102D03* 146 | X0075102Y0078102D03* 147 | X0065102Y0078102D03* 148 | X0065102Y0088102D03* 149 | X0075102Y0088102D03* 150 | X0085102Y0088102D03* 151 | X0085102Y0098102D03* 152 | X0075102Y0098102D03* 153 | X0065102Y0098102D03* 154 | X0065102Y0108102D03* 155 | X0075102Y0108102D03* 156 | X0085102Y0108102D03* 157 | X0085102Y0118102D03* 158 | X0075102Y0118102D03* 159 | X0065102Y0118102D03* 160 | X0065102Y0128102D03* 161 | X0075102Y0128102D03* 162 | X0085102Y0128102D03* 163 | X0085102Y0138102D03* 164 | X0075102Y0138102D03* 165 | X0065102Y0138102D03* 166 | D15* 167 | X0166102Y0137102D03* 168 | X0171102Y0137102D03* 169 | X0176102Y0137102D03* 170 | X0181102Y0137102D03* 171 | X0214102Y0126102D03* 172 | X0214102Y0121102D03* 173 | X0214102Y0116102D03* 174 | X0214102Y0111102D03* 175 | X0214102Y0106102D03* 176 | X0214102Y0101102D03* 177 | X0214102Y0096102D03* 178 | X0199102Y0071102D03* 179 | X0194102Y0071102D03* 180 | X0189102Y0071102D03* 181 | D16* 182 | X0164803Y0064102D03* 183 | X0164803Y0048102D03* 184 | X0164803Y0080102D03* 185 | X0164803Y0096102D03* 186 | D17* 187 | X0177992Y0092362D03* 188 | X0177992Y0099843D03* 189 | X0177992Y0083843D03* 190 | X0177992Y0076362D03* 191 | X0177992Y0067843D03* 192 | X0177992Y0060362D03* 193 | X0177992Y0051843D03* 194 | X0177992Y0044362D03* 195 | D18* 196 | X0209602Y0044535D03* 197 | X0214602Y0044535D03* 198 | X0214602Y0073669D03* 199 | X0209602Y0073669D03* 200 | D19* 201 | X0174661Y0107461D03* 202 | X0169543Y0107461D03* 203 | X0162661Y0107461D03* 204 | X0157543Y0107461D03* 205 | X0160102Y0114744D03* 206 | X0172102Y0114744D03* 207 | D20* 208 | X0150002Y0097126D03* 209 | X0150002Y0093976D03* 210 | X0150002Y0090827D03* 211 | X0150002Y0087677D03* 212 | X0150002Y0084528D03* 213 | X0150002Y0081378D03* 214 | X0150002Y0078228D03* 215 | X0150002Y0075079D03* 216 | X0116202Y0075079D03* 217 | X0116202Y0078228D03* 218 | X0116202Y0081378D03* 219 | X0116202Y0084528D03* 220 | X0116202Y0087677D03* 221 | X0116202Y0090827D03* 222 | X0116202Y0093976D03* 223 | X0116202Y0097126D03* 224 | D21* 225 | X0122079Y0103002D03* 226 | X0125228Y0103002D03* 227 | X0128378Y0103002D03* 228 | X0131528Y0103002D03* 229 | X0134677Y0103002D03* 230 | X0137827Y0103002D03* 231 | X0140976Y0103002D03* 232 | X0144126Y0103002D03* 233 | X0144126Y0069202D03* 234 | X0140976Y0069202D03* 235 | X0137827Y0069202D03* 236 | X0134677Y0069202D03* 237 | X0131528Y0069202D03* 238 | X0128378Y0069202D03* 239 | X0125228Y0069202D03* 240 | X0122079Y0069202D03* 241 | D22* 242 | X0197362Y0129984D03* 243 | X0204843Y0129984D03* 244 | X0204843Y0140221D03* 245 | X0201102Y0140221D03* 246 | X0197362Y0140221D03* 247 | D23* 248 | X0202878Y0107024D03* 249 | X0202878Y0105055D03* 250 | X0202878Y0103087D03* 251 | X0202878Y0101118D03* 252 | X0202878Y0099150D03* 253 | X0202878Y0097181D03* 254 | X0187327Y0097181D03* 255 | X0187327Y0099150D03* 256 | X0187327Y0101118D03* 257 | X0187327Y0103087D03* 258 | X0187327Y0105055D03* 259 | X0187327Y0107024D03* 260 | D24* 261 | X0190181Y0109878D03* 262 | X0192150Y0109878D03* 263 | X0194118Y0109878D03* 264 | X0196087Y0109878D03* 265 | X0198055Y0109878D03* 266 | X0200024Y0109878D03* 267 | X0200024Y0094327D03* 268 | X0198055Y0094327D03* 269 | X0196087Y0094327D03* 270 | X0194118Y0094327D03* 271 | X0192150Y0094327D03* 272 | X0190181Y0094327D03* 273 | D25* 274 | X0149102Y0056102D03* 275 | X0144102Y0056102D03* 276 | X0144102Y0051102D03* 277 | X0149102Y0051102D03* 278 | X0149102Y0046102D03* 279 | X0144102Y0046102D03* 280 | D26* 281 | X0120484Y0047890D02* 282 | X0120484Y0051858D01* 283 | X0115563Y0051858D02* 284 | X0115563Y0047890D01* 285 | X0110642Y0047890D02* 286 | X0110642Y0051858D01* 287 | X0105720Y0051858D02* 288 | X0105720Y0047890D01* 289 | X0127799Y0134346D02* 290 | X0127799Y0138315D01* 291 | X0132720Y0138315D02* 292 | X0132720Y0134346D01* 293 | X0137642Y0134346D02* 294 | X0137642Y0138315D01* 295 | X0142563Y0138315D02* 296 | X0142563Y0134346D01* 297 | X0147484Y0134346D02* 298 | X0147484Y0138315D01* 299 | X0152406Y0138315D02* 300 | X0152406Y0134346D01* 301 | D27* 302 | X0104966Y0073827D02* 303 | X0099238Y0073827D01* 304 | X0099238Y0069102D02* 305 | X0104966Y0069102D01* 306 | X0104966Y0064378D02* 307 | X0099238Y0064378D01* 308 | D28* 309 | X0102092Y0069112D03* 310 | D29* 311 | X0150134Y0127102D03* 312 | X0154071Y0127102D03* 313 | D30* 314 | X0144102Y0121102D03* 315 | X0133102Y0117102D03* 316 | X0131102Y0110102D03* 317 | X0125102Y0110102D03* 318 | X0110102Y0110102D03* 319 | X0109102Y0102102D03* 320 | X0109102Y0095102D03* 321 | X0109102Y0088102D03* 322 | X0094102Y0088102D03* 323 | X0094102Y0095102D03* 324 | X0094102Y0102102D03* 325 | X0094102Y0109102D03* 326 | X0094102Y0116102D03* 327 | X0117102Y0121102D03* 328 | X0151102Y0108102D03* 329 | X0163102Y0102102D03* 330 | X0157102Y0099102D03* 331 | X0157102Y0095102D03* 332 | X0145202Y0090602D03* 333 | X0141702Y0094102D03* 334 | X0131102Y0093102D03* 335 | X0137102Y0084102D03* 336 | X0126102Y0082102D03* 337 | X0132102Y0075102D03* 338 | X0136102Y0076102D03* 339 | X0149102Y0070102D03* 340 | X0154302Y0068902D03* 341 | X0163102Y0070102D03* 342 | X0156102Y0080102D03* 343 | X0163102Y0086102D03* 344 | X0183102Y0095102D03* 345 | X0187102Y0091102D03* 346 | X0204102Y0076102D03* 347 | X0185802Y0067502D03* 348 | X0170402Y0059802D03* 349 | X0163102Y0054102D03* 350 | X0138102Y0062102D03* 351 | X0132102Y0063102D03* 352 | X0134102Y0057102D03* 353 | X0119102Y0060102D03* 354 | X0116102Y0063102D03* 355 | X0110102Y0069102D03* 356 | X0115102Y0057102D03* 357 | X0194102Y0044102D03* 358 | X0205102Y0110102D03* 359 | X0191328Y0114328D03* 360 | X0186102Y0111102D03* 361 | X0178102Y0117102D03* 362 | X0169102Y0118102D03* 363 | X0184102Y0132102D03* 364 | X0201102Y0129102D03* 365 | D31* 366 | X0121102Y0137102D03* 367 | D32* 368 | X0144102Y0097102D03* 369 | M02* 370 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.TXT: -------------------------------------------------------------------------------- 1 | % 2 | M48 3 | M72 4 | T01C0.0140 5 | T02C0.0150 6 | T03C0.0197 7 | T04C0.0200 8 | T05C0.0256 9 | T06C0.0276 10 | T07C0.0400 11 | % 12 | T01 13 | X11010Y6910 14 | X11610Y6310 15 | X11910Y6010 16 | X11510Y5710 17 | X13210Y6310 18 | X13810Y6210 19 | X13410Y5710 20 | X14910Y7010 21 | X15430Y6890 22 | X16310Y7010 23 | X15610Y8010 24 | X16310Y8610 25 | X15710Y9510 26 | X15710Y9910 27 | X16310Y10210 28 | X15110Y10810 29 | X13310Y11710 30 | X14410Y12110 31 | X13110Y11010 32 | X12510Y11010 33 | X11010Y11010 34 | X10910Y10210 35 | X10910Y9510 36 | X10910Y8810 37 | X9410Y8810 38 | X9410Y9510 39 | X9410Y10210 40 | X9410Y10910 41 | X9410Y11610 42 | X11710Y12110 43 | X14170Y9410 44 | X14520Y9060 45 | X13710Y8410 46 | X12610Y8210 47 | X13210Y7510 48 | X13610Y7610 49 | X13110Y9310 50 | X16910Y11810 51 | X17810Y11710 52 | X18610Y11110 53 | X19133Y11433 54 | X20510Y11010 55 | X18310Y9510 56 | X18710Y9110 57 | X20410Y7610 58 | X18580Y6750 59 | X17040Y5980 60 | X16310Y5410 61 | X19410Y4410 62 | X20110Y12910 63 | X18410Y13210 64 | T02 65 | X14410Y9710 66 | T03 67 | X14256Y13633 68 | X13764Y13633 69 | X13272Y13633 70 | X12780Y13633 71 | X14748Y13633 72 | X15241Y13633 73 | X12048Y4987 74 | X11556Y4987 75 | X11064Y4987 76 | X10572Y4987 77 | T04 78 | X12110Y13710 79 | T05 80 | X14410Y5610 81 | X14910Y5610 82 | X14910Y5110 83 | X14410Y5110 84 | X14410Y4610 85 | X14910Y4610 86 | T06 87 | X18910Y7110 88 | X19410Y7110 89 | X19910Y7110 90 | X21410Y9610 91 | X21410Y10110 92 | X21410Y10610 93 | X21410Y11110 94 | X21410Y11610 95 | X21410Y12110 96 | X21410Y12610 97 | X18110Y13710 98 | X17610Y13710 99 | X17110Y13710 100 | X16610Y13710 101 | T07 102 | X6510Y4810 103 | X7510Y4810 104 | X8510Y4810 105 | X8510Y5810 106 | X7510Y5810 107 | X6510Y5810 108 | X6510Y6810 109 | X7510Y6810 110 | X8510Y6810 111 | X8510Y7810 112 | X7510Y7810 113 | X6510Y7810 114 | X6510Y8810 115 | X7510Y8810 116 | X8510Y8810 117 | X8510Y9810 118 | X7510Y9810 119 | X6510Y9810 120 | X6510Y10810 121 | X7510Y10810 122 | X8510Y10810 123 | X8510Y11810 124 | X7510Y11810 125 | X6510Y11810 126 | X6510Y12810 127 | X7510Y12810 128 | X8510Y12810 129 | X8510Y13810 130 | X7510Y13810 131 | X6510Y13810 132 | M30 133 | -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.dri: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.dri -------------------------------------------------------------------------------- /OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.gpi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rollingbug/OneRCFW/a28bbd8d278c1c3832d873411345355b76d83bde/OneRCSchematic/OneRCSchematic_v1/OneRCSchematic_v1_8_Gerber/OneRCSchematic_v1_8.gpi -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # OneRCFW 2 | An Arduino/AVR based flight controller for RC fixed wing. 3 | 4 | 5 | - 5 input channels (Throttle, Ailerons, Elevator, Rudder, flight mode) 6 | - 4 output channels (0.5 us resolution PWM/PPM signal generator). 7 | - Airplane NED attitude tracking function (with 6 DOF sensor, 200 Hz attitude update rate). 8 | - Airplane auto level (roll and pitch) control function. 9 | - Airplane heading lock control function. 10 | - Airplane flight mode control function (manual mode, auto level mode and GPS mode). 11 | - Airplane GPS return to home function (unstabled, still need to do more test!) 12 | - RC output mixer function (for normal airplane, delta wing and V tail). 13 | - 6 DOF sensor calibration function. 14 | - Radio control stick position calibration function. 15 | - Airplane failsafe function. 16 | - Airplane configuration save/reload/reset function. 17 | - Airplane runtime status monitoring function. 18 | 19 | ![PCB picture](OneRCDesignDoc/PCB_block_diagram.png) 20 | 21 | 22 | ![Pin assignment picture](OneRCDesignDoc/arduino_flyctrl_layout_20171217_v1_8.png) 23 | 24 |


25 | 26 | 27 | 28 | 29 | Details: 30 | --------------------- 31 | Flight controller source code: [OneRCFW](https://github.com/rollingbug/OneRCFW/tree/master/OneRCFW) 32 | Flight controller schematic and PCB layout: [OneRCSchematic_v1](https://github.com/rollingbug/OneRCFW/tree/master/OneRCSchematic/OneRCSchematic_v1) 33 | Flight controller test video: [20171029 FC test in very windy (12.5m/s) day.](https://www.youtube.com/watch?v=OjTpQ1Ft-OE) 34 | GUI monitoring tool: [OneRCGUI](https://github.com/rollingbug/OneRCFW/tree/master/OneRCGUI) 35 | Design documents: [OneRCDesignDoc](https://github.com/rollingbug/OneRCFW/tree/master/OneRCDesignDoc) 36 | 37 | ![FC block diagram](OneRCDesignDoc/OneRCFW_block_diagram.png) 38 | 39 | 40 | 41 | 42 | 43 | Build and Installation: 44 | --------------------- 45 | 1. Prepare Arduino 1.6.9 IDE. 46 | 2. Click the **OneRCAirplane.ino** to launch Arduino IDE. 47 | 3. Change board type setting to "Arduino Nano" or "Pro Mini" and correct UART port setting. 48 | 4. Build and upload the firmware to Arduino or customized PCB. 49 | 5. Connect the Radio receiver to flight controller. 50 | 6. Check the channel output signal and the status of on board LEDs.


51 | 52 | 53 | 54 | 55 | For Simulation mode: 56 | --------------------- 57 | 1. Download and instll FlightGear, and copy the protocol and configuration file to speicfic folder. 58 | - Copy [MAVLink.xml](https://github.com/rollingbug/OneRCFW/tree/master/OneRCFlightGearConf/MAVLink.xml) to FlightGear\data\Protocol\ 59 | - Copy [.fgfsrc](https://github.com/rollingbug/OneRCFW/tree/master/OneRCFlightGearConf/.fgfsrc) to C:\Documents and Settings\{User name}\ 60 | 2. Modify the #define IMU_SENSOR_MPU6050 in imu_ctrl.h, change to #define IMU_SENSOR_FG_SIM. 61 | 3. Rebuild and upload the firmware to Arduino. 62 | 4. Modify the COM port setting in both the [MP_fdm_nmea.py](https://github.com/rollingbug/OneRCFW/tree/master/OneRCGUI/MP_fdm_nmea.py) and [MP_fdm_sim.py](https://github.com/rollingbug/OneRCFW/tree/master/OneRCGUI/MP_fdm_sim.py), and lanuch these 2 files. 63 | 5. Lanuch fgfs.exe and enjoy the flight in simulator. 64 | 65 | ![OneRC_FG_Simulator](OneRCDesignDoc/FlightGearSim.png) 66 | 67 | 68 | --------------------------------------------------------------------------------