yuramak/motorcontroldemo_028
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
5812e0c8c9
motorcontroldemo_028/Vsrc/V_TMU.c
Bogdan Kolbov 010df1c0a4 Initial commit
2019-07-29 08:18:57 +03:00

236 lines
8.9 KiB
C
Raw Blame History

/*!
Copyright 2017 ÀÎ "ÍÈÈÝÒ" è ÎÎÎ "ÍÏÔ ÂÅÊÒÎÐ"
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
\file V_TMU.c
\brief Ìîäóëü áëîêà òðèãîíîìåòðè÷åñêèõ ïðåîáðàçîâàíèé (ñì. TTMU, TTMUPark, TTMUIPark)
\author ÎÎÎ "ÍÏÔ Âåêòîð". http://motorcontrol.ru
\version v 1.0 24/06/2019
\addtogroup V_TMU
@{*/
#include "main.h"
//Ôóíêöèÿ ïðåîáðàçîâàíèÿ Ïàðêà. Ïðèíèìàåò íà âõîä àðãóìåíòû â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå àðãóìåíòû îãðàíè÷åíû òîëüêî ñàìèì ôîðìàòîì IQ24.
void TMU_park_calc_IQ24PU (TTMUPark *p) {
TMU->PHIN = _IQ24mpy(((p->ang & 0x00FFFFFF) << 4), _IQ24(2.0*_PI)); //îãðàíè÷èâàåì óãîë åäèíèöåé â IQ24, ïðåîáðàçóåì â IQ28, à çàòåì â àáñîëþòíûé óãîë â ðàäèàíàõ, êàê òðåáóåò òîãî áëîê TMU
TMU->YIN = (p->ds >> 3); //ïðåîáðàçóåì íåîãðàíè÷åííîå ÷èñëî â ôîðìàòå IQ24 â îãðàíè÷åííîå åäèíèöåé â ôîðìàòå IQ28
TMU->XIN = (p->qs >> 3); //òî æå
TMU->CMD = (3 << TMU_CMD_FUNC_Pos) | // Ïîâåðíóòü âåêòîð
(0 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ28
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
p->de = ((int32)TMU->YOUT << 3); //ïðåîáðàçóåì ðåóëüòàò îáðàòíî îò ìàêñèìàëüíî âîçìîæíîé åäèíèöû äëÿ áëîêà TMU â IQ28 äî ìàêñèìàëüíî âîçìîæíîãî ÷èñëà â IQ24
p->qe = ((int32)TMU->XOUT << 3); //òî æå
}
//Ôóíêöèÿ îáðàòíîãî ïðåîáðàçîâàíèÿ Ïàðêà. Ïðèíèìàåò íà âõîä àðãóìåíòû â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå àðãóìåíòû îãðàíè÷åíû òîëüêî ñàìèì ôîðìàòîì IQ24.
void TMU_ipark_calc_IQ24PU (TTMUIPark *p) {
TMU->PHIN = _IQ24mpy(((p->ang & 0x00FFFFFF) << 4), _IQ24(2.0*_PI)); //îãðàíè÷èâàåì óãîë åäèíèöåé â IQ24, ïðåîáðàçóåì â IQ28, à çàòåì â àáñîëþòíûé óãîë â ðàäèàíàõ, êàê òðåáóåò òîãî áëîê TMU
TMU->YIN = (p->qe >> 3); //ïðåîáðàçóåì íåîãðàíè÷åííîå ÷èñëî â ôîðìàòå IQ24 â îãðàíè÷åííîå åäèíèöåé â ôîðìàòå IQ28
TMU->XIN = (p->de >> 3); //òî æå
TMU->CMD = (3 << TMU_CMD_FUNC_Pos) | // Ïîâåðíóòü âåêòîð
(0 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ28
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
p->qs = ((int32)TMU->YOUT << 3); //ïðåîáðàçóåì ðåóëüòàò îáðàòíî îò ìàêñèìàëüíî âîçìîæíîé åäèíèöû äëÿ áëîêà TMU â IQ28 äî ìàêñèìàëüíî âîçìîæíîãî ÷èñëà â IQ24
p->ds = ((int32)TMU->XOUT << 3); //òî æå
}
//Ôóíêöèÿ ñèíóñà. Ïðèíèìàåò íà âõîä óãîë â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå óãîë îãðàíè÷åí òîëüêî ñàìèì ôîðìàòîì IQ24.
_iq TMU_sin_IQ24PU (_iq angle) {
TMU->PHIN = _IQ24mpy(angle & 0x00FFFFFF, _IQ24(2.0*_PI)); //îãðàíè÷èâàåì óãîë åäèíèöåé â IQ24 è ïðåîáðàçóåì â àáñîëþòíûé óãîë â ðàäèàíàõ, êàê òðåáóåò òîãî áëîê TMU
TMU->CMD = (0 << TMU_CMD_FUNC_Pos) | // Âû÷èñëèòü ñèíóñ/êîñèíóñ
(4 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ24
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
return (TMU->YOUT);
}
//Ôóíêöèÿ êîñèíóñà. Ïðèíèìàåò íà âõîä óãîë â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå óãîë îãðàíè÷åí òîëüêî ñàìèì ôîðìàòîì IQ24.
_iq TMU_cos_IQ24PU (_iq angle) {
TMU->PHIN = _IQ24mpy(angle & 0x00FFFFFF, _IQ24(2.0*_PI)); //îãðàíè÷èâàåì óãîë åäèíèöåé â IQ24 è ïðåîáðàçóåì â àáñîëþòíûé óãîë â ðàäèàíàõ, êàê òðåáóåò òîãî áëîê TMU
TMU->CMD = (0 << TMU_CMD_FUNC_Pos) | // Âû÷èñëèòü ñèíóñ/êîñèíóñ
(4 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ24
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
return (TMU->XOUT);
}
//Ôóíêöèÿ àðêòàíãåíñà. Ïðèíèìàåò íà âõîä àðãóìåíòû â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå àðãóìåíòû îãðàíè÷åíû òîëüêî ñàìèì ôîðìàòîì IQ24.
_iq TMU_atan2_IQ24PU (_iq inY, _iq inX) {
TMU->XIN = (inX >> 3); //ïðåîáðàçóåì íåîãðàíè÷åííîå ÷èñëî â ôîðìàòå IQ24 â îãðàíè÷åííîå åäèíèöåé â ôîðìàòå IQ28
TMU->YIN = (inY >> 3); //òî æå
TMU->CMD = (1 << TMU_CMD_FUNC_Pos) | // Ïåðåâîä èç ïðÿìîóãîëüíîé ñèñòåìû êîîðäèíàò â ïîëÿðíóþ
(0 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ28
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
return (_IQ24mpy((((int32)TMU->PHOUT) >> 4), _IQ24(0.15915494309189533576))); //ïðåîáðàçóåì óãîë èç IQ28 â IQ24 è çàòåì äåëèì íà 2Ïè äëÿ ïðåîáðàçîâàíèÿ â îòíîñèòåëüíûé óãîë
}
//Ôóíêöèÿ ïðåîáðàçîâàíèÿ êîîðäèíàò èç ïðÿìîóãîëüíîé ñèñòåìû â ïîëÿðíóþ. Ïðèíèìàåò íà âõîä àðãóìåíòû â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå àðãóìåíòû îãðàíè÷åíû òîëüêî ñàìèì ôîðìàòîì IQ24.
void TMU_cartesianToPolar_calc_IQ24PU (TTMUCarToPol *p) {
TMU->XIN = (p->x >> 3); //ïðåîáðàçóåì íåîãðàíè÷åííîå ÷èñëî â ôîðìàòå IQ24 â îãðàíè÷åííîå åäèíèöåé â ôîðìàòå IQ28
TMU->YIN = (p->y >> 3); //òî æå
TMU->CMD = (1 << TMU_CMD_FUNC_Pos) | // Ïåðåâîä èç ïðÿìîóãîëüíîé ñèñòåìû êîîðäèíàò â ïîëÿðíóþ
(0 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ28
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
p->r = ((int32) TMU->XOUT << 3); //ïðåîáðàçóåì ðåóëüòàò îáðàòíî îò ìàêñèìàëüíî âîçìîæíîé åäèíèöû äëÿ áëîêà TMU â IQ28 äî ìàêñèìàëüíî âîçìîæíîãî ÷èñëà â IQ24
p->phi = _IQ24mpy((((int32)TMU->PHOUT) >> 4), _IQ24(0.15915494309189533576)); //ïðåîáðàçóåì óãîë èç IQ28 â IQ24 è çàòåì äåëèì íà 2Ïè äëÿ ïðåîáðàçîâàíèÿ â îòíîñèòåëüíûé óãîë
}
//Ôóíêöèÿ ïðåîáðàçîâàíèÿ êîîðäèíàò èç ïîëÿðíîé ñèñòåìû â ïðÿìîóãîëüíóþ. Ïðèíèìàåò íà âõîä àðãóìåíòû â îòíîñèòåëüíûõ åäèíèöàõ â IQ24. Ïî âåëè÷èíå àðãóìåíòû îãðàíè÷åíû òîëüêî ñàìèì ôîðìàòîì IQ24.
void TMU_polarToCartesian_calc_IQ24PU (TTMUPolToCar *p) {
TMU->XIN = (p->r >> 3); //ïðåîáðàçóåì íåîãðàíè÷åííîå ÷èñëî â ôîðìàòå IQ24 â îãðàíè÷åííîå åäèíèöåé â ôîðìàòå IQ28
TMU->PHIN = _IQ24mpy(((p->phi & 0x00FFFFFF) << 4), _IQ24(2.0*_PI)); //îãðàíè÷èâàåì óãîë åäèíèöåé â IQ24, ïðåîáðàçóåì â IQ28, à çàòåì â àáñîëþòíûé óãîë â ðàäèàíàõ, êàê òðåáóåò òîãî áëîê TMU
TMU->CMD = (2 << TMU_CMD_FUNC_Pos) | // Ïåðåâîä èç ïîëÿðíîé ñèñòåìû êîîðäèíàò â ïðÿìîóãîëüíóþ
(0 << TMU_CMD_ARGT_Pos) | // Àðãóìåíòû â IQ28
(1 << TMU_CMD_START_Pos); // Ïîåõàëè
//Æäåì, ïîêà áëîê TMU çàêîí÷èò âû÷èñëåíèÿ
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
asm ("nop");
p->x = ((int32) TMU->XOUT << 3) ; //ïðåîáðàçóåì ðåóëüòàò îáðàòíî îò ìàêñèìàëüíî âîçìîæíîé åäèíèöû äëÿ áëîêà TMU â IQ28 äî ìàêñèìàëüíî âîçìîæíîãî ÷èñëà â IQ24
p->y = ((int32) TMU->YOUT << 3); //òî æå
}
/*@}*/
Reference in New Issue View Git Blame Copy Permalink