; model_calculator.asm - калькулятор модели (макро уровень) CH_CALC_CHLIMITS: ; процедура проверки границ каналов LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) ; адреса каналов MOVW X , Z ; сохраним адрес канала LDI YL , low (CH0_MIN) ; LDI YH , high(CH0_MIN) ; значение центров каналов LDI R20 , low (30000) ; значение для расчета LDI R21 , high(30000) LDI R22 , CH_PPM_COL ; количество каналов CH_CALC_CHLIMITS_LOOP: LD R16 , Z+ ; прочитаем рассчитанное значение LD R17 , Z+ ; нашего канала LD R18 , Y+ ; прочитаем минимум канала LD R19 , Y+ ; ADIW Y , 2 ; в Y адрес максимума ADD R18 , R20 ADC R19 , R21 ; увеличим значение минимума на 30000 CALL CP16R16R17VR18R19 ; сравним значение канала и минимума BRCC CH_CALC_CHLIMITS_VERMAX ; проверим максимум канала ; сюда попадаем когда минимум больша значения канала ADIW Y , 2 ; в Y адрес минимума след. канала CH_CALC_CHLIMITS_SAVE_MAX: SUB R18 , R20 ; SBC R19 , R21 ; вычтем 30000 из минимума ST X+ , R18 ; сохраним минимум в канале ST X+ , R19 RJMP CH_CALC_CHLIMITS_GOLOOP CH_CALC_CHLIMITS_VERMAX: LD R18 , Y+ ; прочитаем максимум канала LD R19 , Y+ ; ; в Y адрес минимума следующего канала ADD R18 , R20 ADC R19 , R21 ; увеличим значение максимума на 30000 CALL CP16R16R17VR18R19 ; сравним значение канала и максимума BRCC CH_CALC_CHLIMITS_SAVE_MAX ; значение канала больше максимума ; сюда попадаем когда канал меньше максимума SUB R16 , R20 ; SBC R17 , R21 ; вычтем 30000 из значения канала ST X+ , R16 ; сохраним значение канала ST X+ , R17 CH_CALC_CHLIMITS_GOLOOP: DEC R22 BRNE CH_CALC_CHLIMITS_LOOP RET CH_CALC_TO_PPM: ; оправка данных о каналах на передатчик LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) ; адреса каналов LDI YL , low (CHOUT_LEN) ; адрес значений каналов на вывод LDI YH , high(CHOUT_LEN) ; LDI R16 , CH_PPM_COL ; количество каналов LDI R17 , low (PPM_LEN) LDI R18 , high(PPM_LEN) ; общая длина пакета CH_CALC_TO_PPM_LOOP: LD R19 , Z+ ; скопируем длительность канала LD R20 , Z+ ; ST Y+ , R19 ST Y+ , R20 SUB R17 , R19 ; расчитаем паузу SBC R18 , R20 DEC R16 BRNE CH_CALC_TO_PPM_LOOP ; копируем все каналы ST Y+ , R17 ; сохраним паузу ST Y+ , R18 RET MODEL_CALC: ; расчет значений каналов модели ; очистим значения каналов и заполним их центрами каналов ! LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) ; адреса каналов LDI YL , low (CH0_MID) ; LDI YH , high(CH0_MID) ; значение центров каналов LDI R16 , 6 LDI XL , low (30000) ; LDI XH , high(30000) ; значения каналов по умолчанию 30000 MODEL_CALC_CLEARCH_LOOP: LD R18 , Y+ LD R19 , Y+ ; прочитали центр канала ADD R18 , XL ADC R19 , XH ; сложили со значением по умолчанию ST Z+ , R18 ST Z+ , R19 ADIW Y , 4 ; перешли к следующему центру DEC R16 BRNE MODEL_CALC_CLEARCH_LOOP ; МИКШЕРЫ ; ФЛАПЕРОНЫ ---------------------------------------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_AIL ; CPI R16 , 2 ; есть 2 элерона? BRNE MODEL_CALC_NO_MIX4 ; 2ух элеронов нет, выходим LDS R16 , MODEL_MIX4_ACT CPI R16 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX4 ; нет, выходим ; сюда попадаем когда микшер включен и у нас 2 канала элеронов LDS R17 , MODEL_MIX4_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX4 ; выключатель выключен LDI ZL , low (MODEL_MIX4_1CH1) LDI ZH , high(MODEL_MIX4_1CH1) RCALL MODEL_CALC_MIX_PAR MOVW Y , X MOV R21 , R16 ; в YL, YH, R16 длительности для установки в процентах LDS R20 , MODEL_AIL1 ; канал для первого элерона MOV R19 , YL ; значение левого элерона RCALL MODEL_CALC_SET_LEN LDS R20 , MODEL_AIL2 ; канал для второго элерона MOV R19 , YH ; значение правого элерона RCALL MODEL_CALC_SET_LEN LDS R16 , MODEL_ELE ; у модели есть руль высоты CPI R16 , 0 ; BREQ MODEL_CALC_NO_MIX4 ; руля высоты нет, выходим PUSH R16 LDS R20 , MODEL_ELE1 ; канал для первого руля высоты MOV R19 , R21 ; значение первого руля высоты RCALL MODEL_CALC_SET_LEN POP R16 CPI R16 , 2 BRNE MODEL_CALC_NO_MIX4 ; второго руля высоты нет, выходим LDS R20 , MODEL_ELE2 ; канал для второго руля высоты MOV R19 , R21 ; значение второго руля высоты RCALL MODEL_CALC_SET_LEN MODEL_CALC_NO_MIX4: ; ЭЛЕРОНЫ ----------------------------------------------------------------------------------- LDS R16 , MODEL_AIL ; тип управления по крену CPI R16 , 0 ; проверяем есть ли у модели элероны BREQ MODEL_CALC_NO_AIL ; элеронов нет - переход ! LDI R17 , 0 ; номер UCH_AIL STS UCH_SOURCE_NUM , R17 ; установим орган управления LDI ZL , low (MODEL_AILRATE1) LDI ZH , high(MODEL_AILRATE1) RCALL MODEL_CALC_CH_PAR ; получим процент микширования и экспоненту LDS R17 , MODEL_AIL1 ; канал для первого элерона STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала CPI R16 , 2 ; у нас 2 канала элеронов ? BRNE MODEL_CALC_NO_AIL ; нет, выходим ! ; у нас 2 канала элеронов, обработаем второй LDS R17 , MODEL_AIL2 ; канал для второго элерона STS CH_DEST , R17 LDS R17 , MIX_PROC ; процент микширования ! NEG R17 ; инвертируем процент микширования для элерона STS MIX_PROC , R17 ; процент микширования ! RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_AIL: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; РУЛЬ ВЫСОТЫ -------------------------------------------------------------------------------- LDS R16 , MODEL_ELE ; тип управления по рулю высоты CPI R16 , 0 ; проверяем есть ли у модели руль высоты BREQ MODEL_CALC_NO_ELE ; руля высоты нет - переход ! LDI R17 , 1 ; номер UCH_ELE STS UCH_SOURCE_NUM , R17 ; установим орган управления LDI ZL , low (MODEL_ELERATE1) LDI ZH , high(MODEL_ELERATE1) RCALL MODEL_CALC_CH_PAR ; получим процент микширования и экспоненту LDS R17 , MODEL_ELE1 ; канал для первого руля высоты STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала CPI R16 , 2 ; у нас 2 канала руля высоты ? BRNE MODEL_CALC_NO_ELE ; нет, выходим ! ; у нас 2 канала руля высоты, обработаем второй LDS R17 , MODEL_ELE2 ; канал для второй половины руля высоты STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_ELE: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; РУЛЬ НАПРАВЛЕНИЯ --------------------------------------------------------------------------- LDS R16 , MODEL_RUD ; тип управления по рулю направления CPI R16 , 0 ; проверяем есть ли у модели руль направления BREQ MODEL_CALC_NO_RUD ; руля направления нет - переход ! LDI R17 , 3 ; номер UCH_RUD STS UCH_SOURCE_NUM , R17 ; установим орган управления LDI ZL , low (MODEL_RUDRATE1) LDI ZH , high(MODEL_RUDRATE1) RCALL MODEL_CALC_CH_PAR ; получим процент микширования и экспоненту LDS R17 , MODEL_RUD1 ; канал для первого руля направления STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала CPI R16 , 2 ; у нас 2 канала руля высоты ? BRNE MODEL_CALC_NO_RUD ; нет, выходим ! ; у нас 2 канала руля направления, обработаем второй LDS R17 , MODEL_RUD2 ; канал для второго руля направления STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_RUD: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; ДВИГАТЕЛИ ---------------------------------------------------------------------------------- LDI R16 , 0 ; поставим признак что thr.cut выключен STS MIX_THR_CUT_ON , R16 ; LDS R16 , MODEL_THR ; количество двигателей CPI R16 , 0 ; у нас есть двигатели ? BREQ MODEL_CALC_NO_THR ; обработаем thro.cut если выключатель задан LDS R17 , MODEL_THRCUT ; выключатель thr.cut CALL UCH_SWITCH12_READ_OFF ; читаем значение выключателя BRNE MODEL_CALC_NOTHRCUT ; thr.cut выключатель выключен ; сюда попадаем когда thr.cut включен PUSH R16 LDI R16 , 1 STS MIX_THR_CUT_ON , R16 RJMP MODEL_CALC_NO_THR ; не обрабатываем дальше идем на выход MODEL_CALC_NOTHRCUT: PUSH R16 LDI R16 , 1 STS MIX_CH_ADDFL , R16 LDI YL , low (MODEL_THRRATE) ; LDI YH , high(MODEL_THRRATE) ; LD R17 , Y+ ; STS MIX_PROC_ABS , R17 ; + ; установим процент микширования LD R17 , Y+ ; STS UCH_SOURCE_EXPO , R17 ; + ; установим экспоненту RCALL MODEL_CALC_GET_M_UCH_PAR ; расчитаем параметры стика тяги LDS R16 , MODEL_THR1 ; канал для первого двигателя STS CH_DEST , R16 ; в R16 - номер канала двигателя CALL MODEL_CALC_THRCLC ; расчет параметров канала тяги RCALL CH_CALCULATOR_EXT ; расчитаем значение канала POP R16 CPI R16 , 2 ; у нас есть второй двигатель ? BRNE MODEL_CALC_NO_THR LDS R16 , MODEL_THR2 ; канал для второго двигателя STS CH_DEST , R16 ; в R16 - номер канала двигателя CALL MODEL_CALC_THRCLC ; расчет параметров канала тяги RCALL CH_CALCULATOR_EXT ; расчитаем значение канала MODEL_CALC_NO_THR: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; РЕГУЛЯТОР VAR.A ---------------------------------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_VARIA_ACT ; CPI R16 , 0 ; регулятор активен? BREQ MODEL_CALC_NO_VARA ; нет, идем на выход LDS R17 , MODEL_VARIA_SW ; выключатель CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_VARA ; выключатель выключен PUSH R16 ; сохраним режим LDI R17 , 4 ; номер VAR.A STS UCH_SOURCE_NUM , R17 ; установим орган управления ; получим процент микширования и экспоненту в соответствии с полетным режимом LDI ZL , low (MODEL_VARIA_RATE1) LDI ZH , high(MODEL_VARIA_RATE1) RCALL MODEL_CALC_CH_PAR LDS R17 , MODEL_VARIA_CH1 ; канал 1 STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала POP R16 ; возьмем режим VAR.A CPI R16 , 2 ; режим 2 BREQ MODEL_CALC_VARA_CH2 ; считаем канал 2 CPI R16 , 3 ; режим 3 BRNE MODEL_CALC_NO_VARA ; LDS R16 , MIX_PROC ; у нас диференциальный режим NEG R16 ; MIX_PROC * (-1) STS MIX_PROC , R16 MODEL_CALC_VARA_CH2: ; у нас 2 канала руля направления, обработаем второй LDS R17 , MODEL_VARIA_CH2 ; канал для второго руля направления STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_VARA: ; ; РЕГУЛЯТОР VAR.B---------------------------------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_VARIB_ACT ; CPI R16 , 0 ; регулятор активен? BREQ MODEL_CALC_NO_VARB ; нет, идем на выход LDS R17 , MODEL_VARIB_SW ; выключатель CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_VARB ; выключатель выключен PUSH R16 ; сохраним режим LDI R17 , 5 ; номер VAR.B STS UCH_SOURCE_NUM , R17 ; установим орган управления ; получим процент микширования и экспоненту в соответствии с полетным режимом LDI ZL , low (MODEL_VARIB_RATE1) LDI ZH , high(MODEL_VARIB_RATE1) RCALL MODEL_CALC_CH_PAR LDS R17 , MODEL_VARIB_CH1 ; канал 1 STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала POP R16 ; возьмем режим VAR.A CPI R16 , 2 ; режим 2 BREQ MODEL_CALC_VARB_CH2 ; считаем канал 2 CPI R16 , 3 ; режим 3 BRNE MODEL_CALC_NO_VARB ; LDS R16 , MIX_PROC ; у нас диференциальный режим NEG R16 ; MIX_PROC * (-1) STS MIX_PROC , R16 MODEL_CALC_VARB_CH2: ; у нас 2 канала руля направления, обработаем второй LDS R17 , MODEL_VARIB_CH2 ; канал для второго руля направления STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_VARB: ; ; MIX 1: РВ - ЭЛЕР (Руль высоты на элероны)--------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_ELE ; CPI R16 , 0 ; у нас есть руль высоты ? BREQ MODEL_CALC_NO_MIX1 ; нет руля высоты ! LDS R16 , MODEL_AIL ; CPI R16 , 2 ; есть 2 элерона? BRNE MODEL_CALC_NO_MIX1 ; 2ух элеронов нет, выходим LDS R16 , MODEL_MIX1_ACT CPI R16 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX1 ; нет, выходим ; сюда попадаем когда микшер включен и у нас 2 канала элеронов LDS R17 , MODEL_MIX1_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX1 ; выключатель выключен ; получим процент микширования и экспоненту микшера LDI ZL , low (MODEL_MIX1_1CH1) LDI ZH , high(MODEL_MIX1_1CH1) RCALL MODEL_CALC_MIX_PAR STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , R16 ; экспонента органа управления ! ; в XH значение процента микширования для второго канала PUSH XH LDI R17 , 1 ; номер UCH_ELE STS UCH_SOURCE_NUM , R17 ; установим орган управления ; микширование на первый элерон LDS R17 , MODEL_AIL1 ; канал для первого элерона STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала POP XH STS MIX_PROC , XH ; процент микширования ! ; микширование на второй элерон LDS R17 , MODEL_AIL2 ; канал для второго элерона STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_MIX1: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; MIX 2: ЭЛЕР - 2 РВ (Элероны - 2 Руля высоты)------------------------------------------------ ; проверка возможности работы микшера LDS R16 , MODEL_AIL ; CPI R16 , 0 ; есть элероны? BREQ MODEL_CALC_NO_MIX2 ; элеронов нет, выходим LDS R16 , MODEL_ELE ; CPI R16 , 2 ; есть 2 руля высоты? BRNE MODEL_CALC_NO_MIX2 ; 2ух рулей высоты нет, выходим LDS R16 , MODEL_MIX2_ACT CPI R16 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX2 ; нет, выходим ; сюда попадаем когда микшер включен и у нас 2 канала элеронов LDS R17 , MODEL_MIX2_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX2 ; выключатель выключен, выходим ; получим процент микширования и экспоненту микшера LDI ZL , low (MODEL_MIX2_1CH1) LDI ZH , high(MODEL_MIX2_1CH1) RCALL MODEL_CALC_MIX_PAR STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , R16 ; экспонента органа управления ! ; в XH значение процента микширования для второго канала PUSH XH LDI R17 , 0 ; номер UCH_AIL STS UCH_SOURCE_NUM , R17 ; установим орган управления ; микширование на первый элерон LDS R17 , MODEL_ELE1 ; канал для первого руля высоты STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала POP XH STS MIX_PROC , XH ; процент микширования ! ; микширование на второй элерон LDS R17 , MODEL_ELE2 ; канал для второго руля высоты STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_MIX2: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; ДВИГ.-РВ ----------------------------------------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_THR ; CPI R16 , 0 ; у нас есть двигатели ? BREQ MODEL_CALC_NO_MIX3 ; двигателей нет ! LDS R16 , MODEL_ELE ; CPI R16 , 0 ; у нас есть руль высоты ? BREQ MODEL_CALC_NO_MIX3 ; нет руля высоты ! LDS R17 , MODEL_MIX3_ACT CPI R17 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX3 ; нет, выходим ; сюда попадаем когда микшер включен LDS R17 , MODEL_MIX3_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX3 ; выключатель выключен PUSH R16 ; запомним количество рулей высоты ; получим процент микширования и экспоненту микшера LDI ZL , low (MODEL_MIX3_1CH1) LDI ZH , high(MODEL_MIX3_1CH1) RCALL MODEL_CALC_MIX_PAR STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , R16 ; экспонента органа управления ! ; в XH значение процента микширования для второго канала PUSH XH LDI R17 , 2 ; номер UCH_THR STS UCH_SOURCE_NUM , R17 ; установим орган управления RCALL MODEL_CALC_GET_M_UCH_PAR ; расчитаем параметры стика тяги ; здесь настраиваем канал руля высоты для микширования на него стика тяги LDS R19 , MODEL_ELE1 ; канал для первого руля высоты STS CH_DEST , R19 RCALL CH_CALC_GET_CH_SCALE ; настроим канал для микширования RCALL CH_CALCULATOR_EXT ; микшируем тягу и канал руля высоты POP XH STS MIX_PROC , XH ; процент микширования ! POP R16 ; вспомним количество рулей высоты CPI R16 , 2 ; у нас два руля высоты ? BRNE MODEL_CALC_NO_MIX3 LDS R19 , MODEL_ELE2 ; канал для первого руля высоты STS CH_DEST , R19 RCALL CH_CALC_GET_CH_SCALE ; настроим канал для микширования RCALL CH_CALCULATOR_EXT ; микшируем тягу и канал руля высоты MODEL_CALC_NO_MIX3: ; РН-ЭЛЕР ------------------------------------------------------------------------------------ ; проверка возможности работы микшера LDS R16 , MODEL_RUD ; CPI R16 , 0 ; у нас есть руль направления ? BREQ MODEL_CALC_NO_MIX5 ; нет руля направления ! LDS R16 , MODEL_AIL ; CPI R16 , 0 ; есть элероны? BREQ MODEL_CALC_NO_MIX5 ; элеронов нет, выходим LDS R17 , MODEL_MIX5_ACT CPI R17 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX5 ; нет, выходим ; сюда попадаем когда микшер включен и у нас 2 канала элеронов LDS R17 , MODEL_MIX5_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX5 ; выключатель выключен PUSH R16 ; сохраним количество элеронов ; получим процент микширования и экспоненту микшера LDI ZL , low (MODEL_MIX5_1CH1) LDI ZH , high(MODEL_MIX5_1CH1) RCALL MODEL_CALC_MIX_PAR STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , R16 ; экспонента органа управления ! ; в XH значение процента микширования для второго канала PUSH XH LDI R17 , 3 ; номер UCH_RUD STS UCH_SOURCE_NUM , R17 ; установим орган управления ; микширование на первый элерон LDS R17 , MODEL_AIL1 ; канал для первого элерона STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала POP XH STS MIX_PROC , XH ; процент микширования ! ; микширование на второй элерон POP R16 ; CPI R16 , 2 ; проверим есть ли у нас второй элерон BRNE MODEL_CALC_NO_MIX5 ; второго элерона нет - выходим из микшера LDS R17 , MODEL_AIL2 ; канал для второго элерона STS CH_DEST , R17 RCALL CH_CALCULATOR ; расчитаем значение канала MODEL_CALC_NO_MIX5: ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ; РН-2 ДВИГ. --------------------------------------------------------------------------------- ; проверка возможности работы микшера LDS R16 , MODEL_RUD ; CPI R16 , 0 ; у нас есть руль направления ? BREQ MODEL_CALC_NO_MIX6 ; нет руля направления ! LDS R16 , MODEL_THR ; CPI R16 , 2 ; есть 2 двигателя? BRNE MODEL_CALC_NO_MIX6 ; двигателей нет, выходим LDS R17 , MODEL_MIX6_ACT CPI R17 , 1 ; микшер включен BRNE MODEL_CALC_NO_MIX6 ; нет, выходим LDS R17 , MODEL_MIX6_SW ; выключатель микшера CALL UCH_SWITCH12_READ_ON ; читаем значение выключателя BRNE MODEL_CALC_NO_MIX6 ; выключатель выключен ; сюда попадаем когда микшер включен и у нас 2 канала двигателей ; получим процент микширования и экспоненту микшера LDI ZL , low (MODEL_MIX6_1CH1) LDI ZH , high(MODEL_MIX6_1CH1) RCALL MODEL_CALC_MIX_PAR STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , R16 ; экспонента органа управления ! PUSH XH ; в XH значение процента микширования для второго CH LDI R17 , 3 ; номер UCH_RUD STS UCH_SOURCE_NUM , R17 ; установим орган управления RCALL MODEL_CALC_GET_S_UCH_PAR ; определим параметры симметричного UCH ; на выходе: ; UCH_SIGN - положение стика выше(1)\ниже(-1) центра ; MIX_SCAL_UCH - размер шкалы стика ; MIX_VAL_UCH - значение стика в шкале LDS R16 , MODEL_THR1 ; канал двигателя 1 STS CH_DEST , R16 RCALL CH_CALC_GET_CH_SCALE RCALL MODEL_CALC_THR_SCALE ; определим шкалу канала тяги RCALL CH_CALC_GET_MIX_VAL ; расчитаем значение микширования RCALL CH_CALC_ADD_VAL_TO_CH ; сложим результат со значением микширования POP XH ; процент микширования для второго канала STS MIX_PROC , XH ; процент микширования ! LDS R16 , MODEL_THR2 ; канал двигателя 2 STS CH_DEST , R16 RCALL CH_CALC_GET_CH_SCALE RCALL MODEL_CALC_THR_SCALE ; определим шкалу канала тяги RCALL CH_CALC_GET_MIX_VAL ; расчитаем значение микширования ; изменим знак микшированного результата LDS R16 , MIX_CH_ADDFL CPI R16 , 0 BREQ MODEL_CALC_MIX6_1 LDI R16 , 0 RJMP MODEL_CALC_MIX6_2 MODEL_CALC_MIX6_1: LDI R16 , 1 MODEL_CALC_MIX6_2: STS MIX_CH_ADDFL , R16 RCALL CH_CALC_ADD_VAL_TO_CH ; сложим результат со значением микширования MODEL_CALC_NO_MIX6: RCALL CH_CALC_CHLIMITS ; проверим на границы каналов ; проверим нет ли режима Thr.CUT LDS R16 , MIX_THR_CUT_ON CPI R16 , 1 BRNE MODEL_CALC_MIX_END ; режима нет, выходим ; установка значения глушения для каналов двигателя LDS R19 , MODEL_THRCUTV ; значение глушения LDS R20 , MODEL_THR1 ; канал для первого двигателя RCALL MODEL_CALC_SET_LEN POP R16 ; количество двигателей запоминали на стеке ; в процедуре микширования двигателей CPI R16 , 2 BRNE MODEL_CALC_THR_CUT_END LDS R16 , MODEL_THR2 ; канал для второго двигателя LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) CALL SYP_MUL2R16_ADDZ ST Z+ , XL ST Z+ , XH MODEL_CALC_THR_CUT_END: MODEL_CALC_MIX_END: ; ОБРАБОТКА МИКШЕРОВ ЗАВЕРШЕНА --------------------------------------------------------------- ; проверяем необходимость передачи данных симулятору RCALL CH_CALC_TO_PPM ; оправка данных о каналах на передатчик LDS R16 , UART_MODE ; режим работы UART CPI R16 , 1 ; BRNE MODEL_CALC_MIX_RET ; если режим 0 - команды, то выход RCALL CH_CALC_SEND_SIMM ; процедура отправки данных на симулятор MODEL_CALC_MIX_RET: RET ; -------------------------------------------------------------------------------------------- ; | ВСПОМОГАТЕЛЬНЫЕ ПРОЦЕДУРЫ ; -------------------------------------------------------------------------------------------- ; +++ процедура одобрена ! MODEL_CALC_GET_S_UCH_PAR: ; процедура получения параметров симметричного UCH ; определяются следующие параметры: ; UCH_SIGN - положение стика выше(1)\ниже(-1) центра ; MIX_SCAL_UCH - размер шкалы стика ; MIX_VAL_UCH - значение стика в шкале ; PUSH R17 PUSH R18 LDS R16 , UCH_SOURCE_NUM ; получили номер UCH CALL SYP_GET_VALX_ADCR16 ; получим значение UCH ; в X значение UCH ; прочитаем центр UCH LDI R17 , 1 ; читаем в Y CALL SYP_GET_MID_R17XY_ADC_R16_CALIBR ; в Y значение центра UCH CALL CP16XY ; сравним X и Y BRSH MODEL_CALC_GSUP_SIGN1 ; наш стик выше центра ; сюда попадаем когда стик ниже центра PUSH YL ; сохраним центр PUSH YH ; ; SUB YL , XL ; расчитаем значение стика от центра! SBC YH , XH ; STS MIX_VAL_UCH , YL ; STS MIX_VAL_UCH+1 , YH ; сохраним значение стика POP XH ; вспомнили центр но, уже в X POP XL ; LDI R18 , -1 ; признак стика ниже центра LDI R17 , 1 CALL SYP_GET_MIN_R17XY_ADC_R16_CALIBR ; в Y минимум ; здесь: ; X максимум ; Y минимум MODEL_CALC_GSUP_CALC_SC_VAL:; расчитываем шкалу и значение ADC STS UCH_SIGN , R18 SUB XL , YL ; вычтем из максимума минимум SBC XH , YH ; получим шкалу UCH STS MIX_SCAL_UCH , XL STS MIX_SCAL_UCH+1 , XH ; сохраним шкалу UCH POP R18 POP R17 RET MODEL_CALC_GSUP_SIGN1: ; сюда попадаем когда стик выше центра ; значение стика в X ; Y уже минимум LDI R18 , 1 ; признак стика выше центра SUB XL , YL ; из значения стика вычтем центр SBC XH , YH ; чтобы получить значение стика STS MIX_VAL_UCH , XL ; относительно центра STS MIX_VAL_UCH+1 , XH ; сохраним значение стика LDI R17 , 0 CALL SYP_GET_MAX_R17XY_ADC_R16_CALIBR ; в X максимум ; здесь: ; X максимум ; Y минимум RJMP MODEL_CALC_GSUP_CALC_SC_VAL ; -------------------------------------------------------------------------------------------- ; +++ процедура одобрена ! MODEL_CALC_GET_M_UCH_PAR:; установка параметров UCH имеющим один диапазон ; (от мин до макс) без центрального положения (например для стика тяги) ; ; необходимо - потому что стик работает от нижнего до верхнего положения ; без середины (в отличие от других стиков управления) ; ; входных параметров нет ; устанавливаем на выходе: ; MIX_SCAL_UCH - размер шкалы стика тяги ; MIX_VAL_UCH - положение стика (без минимума) ; PUSH R16 PUSH R17 LDI ZL , low (ADC2_MIN) LDI ZH , high(ADC2_MIN) LD XL , Z+ ; LD XH , Z+ ; читаем минимум ADIW Z , 2 ; пропустим центр LD YL , Z+ ; LD YH , Z+ ; читаем максимум SUB YL , XL SBC YH , XH ; в Y шкала органа управления STS MIX_SCAL_UCH , YL STS MIX_SCAL_UCH+1 , YH ; + ; сохраним шкалу органа управления LDS YL , ADCX+2*2 ; третий по счету ADC - UCH тяги LDS YH , ADCX+2*2+1 ; SUB YL , XL SBC YH , XH ; уменьшим значение UCH на минимум STS MIX_VAL_UCH , YL STS MIX_VAL_UCH+1 , YH ; + ; сохраним значение UCH LDI R16 , 1 ; стик находиться выше центра STS UCH_SIGN , R16 ; (поскольку считаем что центр это минимум) POP R17 POP R16 RET ;--------------------------------------------------------------------------------------------- MODEL_CALC_SET_LEN: ; установка длительности канала по процентному значению ; длительность берем с точкой отсчета 30000 мкс (для последующих операций) ; ; входные параметры: ; R19 - процентное значение длительности канала ; R20 - номер канала для установки значения ; устанавливаем на выходе: ; CH_VAL - для соответствующего канала, будет равен длительности канала ; в абсолютных единицах (мкс+30000) ; PUSH R20 CALL CH_LEN_CALC ; расчитаем длительность ; в R16:R17 длительность LDI XL , low (30000) LDI XH , high(30000) ADD XL , R16 ADC XH , R17 ; в X итоговая длительность POP R16 ; берем номер канала LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) CALL SYP_MUL2R16_ADDZ ST Z+ , XL ; сохраняем длительность канала ST Z+ , XH RET ; -------------------------------------------------------------------------------------------- MODEL_CALC_THRCLC: ; расчет параметров каналов тяги для микширования ; в R16 номер канала RCALL MODEL_CALC_THR_SCALE ; расчет шкалы канала тяги ; в Y минимум канала ; установим первоначальное значение канала для канала тяги LDI ZL , low (CH_VAL) LDI ZH , high(CH_VAL) CALL SYP_MUL2R16_ADDZ LDI XL , low (30000) ; минимальное значение LDI XH , high(30000) ; ADD XL , YL ; минимум канала ADC XH , YH ; ST Z+ , XL ST Z+ , XH RET MODEL_CALC_THR_SCALE: ; расчет шкалы канала тяги ; номер канала в R16 LDI ZL , low (CH0_MIN) ; минимум LDI ZH , high(CH0_MIN) ; CALL SYP_MUL6R16_ADDZ_LDXLXH MOVW Y , X ; в Y минимум канала LDI ZL , low (CH0_MAX) ; максимум LDI ZH , high(CH0_MAX) ; CALL SYP_MUL6R16_ADDZ_LDXLXH ; в X максимум канала SUB XL , YL SBC XH , YH ; в X шкала канала тяги STS MIX_SCAL_CH , XL ; STS MIX_SCAL_CH+1 , XH ; + ; сохраним шкалу тяги ; на выходе в Y минимум канала RET ; -------------------------------------------------------------------------------------------- MODEL_CALC_CH_PAR:; получение процента микширования и экспоненты для руля ; вход: Z адрес процентов первого полетного режима PUSH R16 LDS R16 , FMODE_NUM ; прочитаем номер полетного режима CALL SYP_MUL2R16_ADDZ_LDXLXH STS MIX_PROC , XL ; процент микширования ! STS UCH_SOURCE_EXPO , XH ; экспонента органа управления ! POP R16 RET ; -------------------------------------------------------------------------------------------- MODEL_CALC_MIX_PAR: ; получение процента микширования и экспоненты для руля ; вход: Z адрес процентов первого полетного режима PUSH R17 LDS R16 , FMODE_NUM ; прочитаем номер полетного режима LDI R17 , 3 CALL SYP_MULR16R17_ADDZ LD XL , Z+ ; канал 1 LD XH , Z+ ; канал 2 LD R16 , Z+ ; экспо POP R17 RET ; процедура расчета значения канала ! -------------------------------------------------------- CH_CALCULATOR_EXT: ; внешний вход в процедуру расчета значения канала ; без расчета значений шкал PUSH R16 RJMP CH_CALCULATOR_EXT1 CH_CALCULATOR: ; основной вход в процедуру расчета значения канала (с расчетом шкал) PUSH R16 ; процедура микширования RCALL MODEL_CALC_GET_S_UCH_PAR ; расчитаем шкалу симметричного UCH RCALL CH_CALC_GET_CH_SCALE ; расчитаем шкалу CH CH_CALCULATOR_EXT1: RCALL CH_CALC_GET_MIX_VAL ; получим значение для микширования RCALL CH_CALC_ADD_VAL_TO_CH ; прибавим полученное значение микширования ; к значению канала POP R16 RET CH_CALC_GET_MIX_VAL: ; процедура расчета значения для микширования LDS R16 , MIX_SCAL_UCH ; берем шкалу органа управления LDS R17 , MIX_SCAL_UCH+1 ; CALL MATCH_SET_P1 ; устанавливает как параметр 1 LDI R16 , 100 ; 100 LDI R17 , 0 ; CALL MATCH_SET_P2 ; устанавливаем как параметр 2 CALL MATCH_MUL32 ; UCH_SC * 100 CALL MATCH_MOV_RES_M1 ; запомним результат в М1 LDS R16 , MIX_VAL_UCH ; значение органа управления LDS R17 , MIX_VAL_UCH+1 ; VAL_UCH CALL MATCH_SET_P1 ; устанавливаем как параметр 1 LDS R16 , MIX_SCAL_CH ; шкала выходного канала LDS R17 , MIX_SCAL_CH+1 ; SCAL_CH CALL MATCH_SET_P2 ; устанавливаем как параметр 2 CALL MATCH_MUL32 ; SCAL_CH * VAL_UCH CALL MATCH_MOV_RES_P1 ; запомним результат в P1 LDS R16 , MIX_PROC_ABS ; Процент микширования LDI R17 , 0 ; CALL MATCH_SET_P2 ; устанавливаем как параметр 2 CALL MATCH_MUL32 ; SCAL_CH * VAL_UCH * PROC CALL MATCH_MOV_RES_P1 ; запомним результат в P1 CALL MATCH_MOV_M1_RES CALL MATCH_MOV_RES_P2 CALL MATCH_DIV32 ; (SCAL_CH * VAL_UCH * PROC)/(UCH_SC * 100) LDS R16 , CH_DEST ; расчитаем адрес данных длительности LDI ZL , low (CH_VAL) ; канала LDI ZH , high(CH_VAL) ; CALL SYP_MUL2R16_ADDZ ; ; Z адрес для сохранения данных микширования RET CH_CALC_ADD_VAL_TO_CH: ; процедура прибавления значения микширования к каналу ; в Z адрес для сохранения данных микширования MOVW Y , Z LD XL , Z+ ; прочитаем значение канала LD XH , Z CALL MATCH_GET_RES ; получим результат операций - длительность ! LDS R18 , MIX_CH_ADDFL ; признак необходимости сложения\вычитания CPI R18 , 1 BRNE CH_CALC_SUB_CHLEN ; вычитаем ! ; сюда попадаем когда нужно сложить значение с длительностью канала ADD XL , R16 ADC XH , R17 RJMP CH_CALC_SAVE_CHLEN CH_CALC_SUB_CHLEN: ; сюда попадаем когда нужно вычесть значение из длительности канала SUB XL , R16 SBC XH , R17 CH_CALC_SAVE_CHLEN: ; сохраняем результирующее значение канала MOVW Z , Y ST Z+ , XL ST Z , XH RET ;--------------------------------------------------------------------------------------------- ; +++ процедура одобрена ! CH_CALC_GET_CH_SCALE: ; процедура расчета значения шкалы канала получателя для микширования ; до вызова этой процедуры должна быть вызвана процедура расчета ; шкал и значения UCH (MODEL_CALC_GET_S_UCH_PAR) ; LDS R18 , MIX_PROC CALL SYP_GET_CH_REVERSE ; получим значение реверса BRNE CH_CALC_GET_CH_SCALE1 ; нет реверса NEG R18 ; есть реверс, поменяем знак у процента микширования CH_CALC_GET_CH_SCALE1: ; в R18 значение процента микширования с учетом реверса LDS R19 , UCH_SIGN ; возьмем положение UCH SBRC R18 , 7 ; если MIX_PROC<0 NEG R19 ; инвертируем UCH_SIGN SBRC R18 , 7 ; если MIX_PROC<0 NEG R18 ; инвертируем MIX_PROC STS MIX_PROC_ABS , R18 ; сохраним модуль процента микширования ; в R19 шкала канала для загрузки ; -1 - нужно загрузить нижнюю часть канала ; 1 - нужно загрузить верхнюю часть канала LDS R16 , CH_DEST ; берем номер канала LDI R17 , 0 ; рeзультат в X CALL SYP_GET_MID_R17XY_CHR16 ; прочитаем центр канала CPI R19 , 1 ; грузим верхнюю часть шкалы ? BRNE CH_CALC_GET_CH_SCALE2 ; нет, грузим нижнюю часть шкалы ; грузим положительную шкалу канала LDI R17 , 1 ; рeзультат в Y CALL SYP_GET_MAX_R17XY_CHR16 ; прочитаем максимум канала CH_CALC_GET_CH_SCALE3: ; вычисляем шкалу канала SUB YL , XL ; вычтем из большего меньшее SBC YH , XH ; для получения шкалы канала STS MIX_SCAL_CH , YL ; STS MIX_SCAL_CH+1 , YH ; сохраним шкалу канала STS MIX_CH_ADDFL , R19 ; сохраним признак действия ; с результатом микширования RET CH_CALC_GET_CH_SCALE2: ; грузим отрицательную шкалу канала MOVW Y , X ; переместим наш центр в Y LDI R17 , 0 ; рeзультат в X CALL SYP_GET_MIN_R17XY_CHR16 ; прочитаем минимум канала LDI R19 , 0 ; будем вычитать результат микширования RJMP CH_CALC_GET_CH_SCALE3 CH_CALC_SEND_SIMM: ; процедура отправки данных на симулятор LDI ZL , low (CHOUT_LEN) LDI ZH , high(CHOUT_LEN) ; данные о каналах LDI YL , low (UART_BUFF) LDI YH , high(UART_BUFF); адрес буфера на передачу STS UART_ADR , YL ; сохраним адрес первого байта для STS UART_ADR+1 , YH ; передачи LDI R16 , 0x55 ; отправка заголовка ST Y+ , R16 LDI R16 , 0xFC ST Y+ , R16 LDI XL , 0 ; контрольная сумма LDI XH , 0 ; LDI R16 , 0 ; вычислительный ноль LDI R17 , 6 ; количество каналов на передачу CH_CALC_SEND_SIMM_LOOP: LD R18 , Z+ ; прочитали канал LD R19 , Z+ ; ST Y+ , R19 ; записали канал ST Y+ , R18 ; ADD XL , R18 ; прибавим младший байт для CRC ADC XH , R16 ADD XL , R19 ; прибавим старший байт для CRC ADC XH , R16 DEC R17 BRNE CH_CALC_SEND_SIMM_LOOP ; сюда попадаем когда скопировали данные всех каналов LDI ZL , low (CHOUT_LEN+4) LDI ZH , high(CHOUT_LEN+4) ; адрес данных о канале 3 LD R20 , Z+ LD R21 , Z+ ; данные канала 3 LDI R18 , low (2044) LDI R19 , high(2044) SUB R18 , R20 SBC R19 , R21 ; 2044-CH3 ST Y+ , R19 ; записали канал3 в доп. формате ST Y+ , R18 ; ADD XL , R18 ; прибавим младший байт для CRC ADC XH , R16 ADD XL , R19 ; прибавим старший байт для CRC ADC XH , R16 ST Y+ , XH ; записали контрольную сумму ST Y+ , XL ; LDI R16 , 18 ; количество байт для передачи STS UART_TX_COL , R16 LDI R16 , 1 STS UART_TX_MODE , R16 ; в состояние передачи данных LDI R16 , (1<