 |
| ピンぼけじゃないもん影だもん（趣味：写真） |
■Energia MT=マルチタスク■
EnergiaはArduino互換の開発環境ですが、MSP432P401R版はマルチタスクに対応しています。マルチタスクというにはあまりにも単純でセマフォとかメールとかまったくありません。でも、今までのArduinoと比べれば格段に作りやすくなりました。
使い方は単純で
- Energia IDEでタブを開き適当な名前を付ける
- メイン同様setupとloop関数を用意するが、setupDisplay(), loopDisplay()のようにタスク名が後に続くようにするとsetup+loopのペアを別タスクとして認識する
- メインに置いたヘッダやメイン上のGlobal変数は別タスクでも認識される
排他などについては厳密な排他制御は無理、暫定的にGlobal上にフラグを置いて自分で制御するしかありません。また、プリエンプションやタスク優先度がどうなっているのかも不明。その辺まったく情報がありません。まぁそのうち何とかなるでしょう。
ということで、とりあえず毎度お馴染みのスペクトルアナライザーをマルチタスクで作ってみました。
■材料
MSP432P401R、アンプ付きマイクAE-MICAMP、超小型グラフィック液晶AQM1284A、ブレッドボード、ジャンパ線…すべて秋月で揃います。合計4000円ぐらい？ USBシリアルなども不要なのでArduino uno正規品を買うより安いです。
マイクはアナログ出力なのでドライバ等不要です。液晶についてはAQM1284AのコントローラIC ST7565RのライブラリをAdafruitさんが公開しているので、それを少し改造して使います。
FFTは大浦先生のライブラリそのままですが、M4F対応としてdoubleをfloatに置換して使用します。
以上です。
■配線■
マイクはGND, Vcc（5vが推奨されていますが出力電圧が大きくなりすぎるので3.3vにします）を接続し、OUTをEnergiaのP5.0に接続します。
AQM1284Aは以下の通りです。
| AQM1284A | Energia MSP432P401R |
| VDD | 3V3 |
| CS | P3.0 / 18 |
| RESET | P2.5 / 19 |
| RS | P5.7 / 17 |
| SCLK | P1.5 / 7 |
| SDI | P1.6 / 15 |
| GND | GND |
■使い方■
各ソースは末尾に掲載します。長くてすみません。
Energiaは最新の16を使用してください。起動する前に、ST7565という名前のフォルダを作り、その中にST7565.cppとST7565.hを入れ、Energiaのlibraryフォルダに入れます。
Enegiaを起動したら、新しいプロジェクトを作り、FFTGraph_MTをコピペします。次に、ウィンドウ右上にある下向き三角をクリックして「新規タブ」を選び、TaskLCDという名前を付けてTaskLCDをコピペします（名前は何でも良いです）。
配線の点検をすませたらMSP432をマイクロUSBで接続し、Energiaでcmd+M、ビルドとプログラムの転送が終わるとグラフィック液晶に表示が出る…はずです。
なお、この作り方は厳密にいえば正しいマルチタスクではありません。TaskLCDが表示している最中にメインスレッドが表示バッファを書き換えてしまうこともありえます。でも、表示中にスペクトルが変わったとしても人間の目で追える変化ではないので、大目に見てやってください。
なお、この作り方は厳密にいえば正しいマルチタスクではありません。TaskLCDが表示している最中にメインスレッドが表示バッファを書き換えてしまうこともありえます。でも、表示中にスペクトルが変わったとしても人間の目で追える変化ではないので、大目に見てやってください。
■毎度お馴染みのハマり■
Adafruitのライブラリの移植は、コンパイルエラーが出る→つぶす→ビルド→エラー→潰す…を淡々と繰り返すだけですが、それで終わらないのがコントラスト設定とpagemapでした。
コントラスト設定は秋月版との液晶の違いかAdafruit版だと真っ黒になってしまうのですが、Voltage Resistor Setを指定して回避できました。
pagemapはコントローラ上のページと液晶の表示位置を対応させるための表ですが、配布されているものと秋月のでは仕様が異なるようで、こんなのが出ました。左上隅から右下隅への直線がちょっと下にずれてます。64ドットで8ページ、たぶん2ページずれているのでしょう。
何度かの途中試行錯誤を経て
 |
| 逆だ |
 |
| なんだこれ |
最終的にちゃんと表示が出ました。ここまで約1時間。
その次はマイクからの入力が常にゼロという現象が。そんなお行儀のいいアナログ入力はありえないのでしばらく頭を抱えていたのですが、これはanalogRead対応のピンにつないだつもりがanalogWriteのピンだった、というオチ。そりゃ値ゼロだわorz
FFTライブラリはつい最近使ったものなので問題なし。マルチタスク対応に関しては、奇跡的にも一発で動きました。何も難しいことしていないので当たり前ですけども、簡単なところで引っかかるのがこの私orz
でも、「自作補聴器」に今までで一番近づいたなぁ。すごいぞM4F、偉いぞM4F。
でも、「自作補聴器」に今までで一番近づいたなぁ。すごいぞM4F、偉いぞM4F。
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| /* | |
| $Id:$ | |
| ST7565 LCD library! | |
| Copyright (C) 2010 Limor Fried, Adafruit Industries | |
| This library is free software; you can redistribute it and/or | |
| modify it under the terms of the GNU Lesser General Public | |
| License as published by the Free Software Foundation; either | |
| version 2.1 of the License, or (at your option) any later version. | |
| This library is distributed in the hope that it will be useful, | |
| but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
| Lesser General Public License for more details. | |
| You should have received a copy of the GNU Lesser General Public | |
| License along with this library; if not, write to the Free Software | |
| Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA | |
| // some of this code was written by <cstone@pobox.com> originally; it is in the public domain. | |
| // this source is modified for AQM1284 by kkurahashi | |
| */ | |
| #include <stdlib.h> | |
| #include "ST7565.h" | |
| #define ST7565_STARTBYTES 1 | |
| #define _BV(bit) (1 << (bit)) | |
| unsigned char is_reversed = 0; | |
| // a handy reference to where the pages are on the screen | |
| const unsigned char pagemap[] = { 5, 4, 3, 2, 1, 0, 7, 6}; | |
| // a 5x7 font table | |
| const extern unsigned char font[]; | |
| // the memory buffer for the LCD | |
| unsigned char st7565_buffer[1024] = { | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x3, 0x7, 0xF, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F, 0x3F, 0x7, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x7F, 0x3F, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x1F, 0x3F, 0x70, 0x70, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x6, 0x6, 0x0, 0x0, 0x0, 0x3, 0x3, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0xF, 0x7, 0x7, | |
| 0x7, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3E, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xF, 0x3F, | |
| 0x70, 0x60, 0x60, 0x60, 0x60, 0x30, 0x7F, 0x3F, 0x0, 0x0, 0x1F, 0x3F, 0x70, 0x60, 0x60, 0x60, | |
| 0x60, 0x39, 0xFF, 0xFF, 0x0, 0x6, 0x1F, 0x39, 0x60, 0x60, 0x60, 0x60, 0x30, 0x3F, 0x7F, 0x0, | |
| 0x0, 0x60, 0xFF, 0xFF, 0x60, 0x60, 0x0, 0x7F, 0x7F, 0x70, 0x60, 0x60, 0x40, 0x0, 0x7F, 0x7F, | |
| 0x0, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x60, 0xFF, 0xFF, | |
| 0x60, 0x60, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x80, 0xF8, 0xFC, 0xFE, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xEF, 0xE7, 0xE7, 0xE3, | |
| 0xF3, 0xF9, 0xFF, 0xFF, 0xFF, 0xF7, 0x7, 0x1F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, | |
| 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x7F, 0x3F, 0x3F, 0x1F, 0xF, 0x7, 0x3, 0x0, 0x0, 0x0, 0xC0, | |
| 0xE0, 0x60, 0x20, 0x20, 0x60, 0xE0, 0xE0, 0xE0, 0x0, 0x0, 0x80, 0xC0, 0xE0, 0x60, 0x20, 0x60, | |
| 0x60, 0xE0, 0xE0, 0xE0, 0x0, 0x0, 0x80, 0xC0, 0x60, 0x60, 0x20, 0x60, 0x60, 0xE0, 0xE0, 0x0, | |
| 0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x80, 0xE0, | |
| 0x60, 0x60, 0x60, 0x60, 0xE0, 0x80, 0x0, 0x0, 0x0, 0xE0, 0xE0, 0x0, 0x0, 0x0, 0xE0, 0xE0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x3, 0x7, 0x1F, 0x9F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD, 0xF1, 0xE3, | |
| 0xE3, 0xCF, 0xFF, 0xFF, 0xFF, 0xFF, 0xF0, 0xFC, 0x7F, 0x3F, 0x3F, 0x3F, 0x3F, 0x7F, 0xFF, 0xFF, | |
| 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFC, 0xF0, 0xE0, 0x80, 0x0, 0x0, 0x0, 0xC, | |
| 0x1C, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x7F, 0x7F, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x7, 0x7, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x1C, 0xC, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x7, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xFE, 0xFE, 0xFE, 0xFC, 0xF8, | |
| 0xF8, 0xF0, 0xFE, 0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xFF, | |
| 0xFF, 0x0, 0x0, 0x0, 0xFF, 0xFF, 0xE0, 0xC0, 0xC0, 0xC0, 0xFF, 0x7F, 0x0, 0x0, 0x1E, 0x7F, | |
| 0xE1, 0xC0, 0xC0, 0xC0, 0xC0, 0x61, 0xFF, 0xFF, 0x0, 0x0, 0xFE, 0xFF, 0x1, 0x0, 0x0, 0x0, | |
| 0xFF, 0xFF, 0x0, 0x0, 0x21, 0xF9, 0xF8, 0xDC, 0xCC, 0xCF, 0x7, 0x0, 0xC0, 0xFF, 0xFF, 0xC0, | |
| 0x80, 0x0, 0xFF, 0xFF, 0xC0, 0xC0, 0x80, 0x0, 0x0, 0xFF, 0xFF, 0x0, 0x0, 0x1F, 0x7F, 0xF9, | |
| 0xC8, 0xC8, 0xC8, 0xC8, 0x79, 0x39, 0x0, 0x0, 0x71, 0xF9, 0xD8, 0xCC, 0xCE, 0x47, 0x3, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x80, 0x80, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xF8, 0xFC, 0xFC, 0xFC, 0xFC, 0xF8, 0xF0, 0xC0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, | |
| 0xC0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80, | |
| 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0, | |
| 0xC0, 0x80, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, | |
| 0x0, 0x0, 0xC0, 0x80, 0x0, 0x0, 0x0, 0x0, 0x0, 0xC0, 0xC0, 0x0, 0x0, 0x0, 0x80, 0xC0, | |
| 0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0x80, 0x0, 0x0, 0x80, 0xC0, 0xC0, 0xC0, 0xC0, 0x80, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, | |
| 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,}; | |
| // reduces how much is refreshed, which speeds it up! | |
| // originally derived from Steve Evans/JCW's mod but cleaned up and | |
| // optimized | |
| // #define enablePartialUpdate | |
| #ifdef enablePartialUpdate | |
| static unsigned char xUpdateMin, xUpdateMax, yUpdateMin, yUpdateMax; | |
| #endif | |
| static void updateBoundingBox(unsigned char xmin, unsigned char ymin, unsigned char xmax, unsigned char ymax) { | |
| #ifdef enablePartialUpdate | |
| if (xmin < xUpdateMin) xUpdateMin = xmin; | |
| if (xmax > xUpdateMax) xUpdateMax = xmax; | |
| if (ymin < yUpdateMin) yUpdateMin = ymin; | |
| if (ymax > yUpdateMax) yUpdateMax = ymax; | |
| #endif | |
| } | |
| void ST7565::drawbitmap(unsigned char x, unsigned char y, | |
| const unsigned char *bitmap, unsigned char w, unsigned char h, | |
| unsigned char color) { | |
| for (unsigned char j=0; j<h; j++) { | |
| for (unsigned char i=0; i<w; i++ ) { | |
| if (pgm_read_byte(bitmap + i + (j/8)*w) & _BV(j%8)) { | |
| my_setpixel(x+i, y+j, color); | |
| } | |
| } | |
| } | |
| updateBoundingBox(x, y, x+w, y+h); | |
| } | |
| void ST7565::drawstring(unsigned char x, unsigned char line, char *c) { | |
| while (c[0] != 0) { | |
| drawchar(x, line, c[0]); | |
| c++; | |
| x += 6; // 6 pixels wide | |
| if (x + 6 >= LCDWIDTH) { | |
| x = 0; // ran out of this line | |
| line++; | |
| } | |
| if (line >= (LCDHEIGHT/8)) | |
| return; // ran out of space :( | |
| } | |
| } | |
| void ST7565::drawstring_P(unsigned char x, unsigned char line, const char *str) { | |
| while (1) { | |
| char c = pgm_read_byte(str++); | |
| if (! c) | |
| return; | |
| drawchar(x, line, c); | |
| x += 6; // 6 pixels wide | |
| if (x + 6 >= LCDWIDTH) { | |
| x = 0; // ran out of this line | |
| line++; | |
| } | |
| if (line >= (LCDHEIGHT/8)) | |
| return; // ran out of space :( | |
| } | |
| } | |
| void ST7565::drawchar(unsigned char x, unsigned char line, char c) { | |
| for (unsigned char i =0; i<5; i++ ) { | |
| st7565_buffer[x + (line*128) ] = pgm_read_byte(font+(c*5)+i); | |
| x++; | |
| } | |
| updateBoundingBox(x, line*8, x+5, line*8 + 8); | |
| } | |
| // bresenham's algorithm - thx wikpedia | |
| void ST7565::drawline(unsigned char x0, unsigned char y0, unsigned char x1, unsigned char y1, | |
| unsigned char color) { | |
| unsigned char steep = abs(y1 - y0) > abs(x1 - x0); | |
| if (steep) { | |
| swap(x0, y0); | |
| swap(x1, y1); | |
| } | |
| if (x0 > x1) { | |
| swap(x0, x1); | |
| swap(y0, y1); | |
| } | |
| // much faster to put the test here, since we've already sorted the points | |
| updateBoundingBox(x0, y0, x1, y1); | |
| unsigned char dx, dy; | |
| dx = x1 - x0; | |
| dy = abs(y1 - y0); | |
| int8_t err = dx / 2; | |
| int8_t ystep; | |
| if (y0 < y1) { | |
| ystep = 1; | |
| } else { | |
| ystep = -1;} | |
| for (; x0<=x1; x0++) { | |
| if (steep) { | |
| my_setpixel(y0, x0, color); | |
| } else { | |
| my_setpixel(x0, y0, color); | |
| } | |
| err -= dy; | |
| if (err < 0) { | |
| y0 += ystep; | |
| err += dx; | |
| } | |
| } | |
| } | |
| // filled rectangle | |
| void ST7565::fillrect(unsigned char x, unsigned char y, unsigned char w, unsigned char h, | |
| unsigned char color) { | |
| // stupidest version - just pixels - but fast with internal buffer! | |
| for (unsigned char i=x; i<x+w; i++) { | |
| for (unsigned char j=y; j<y+h; j++) { | |
| my_setpixel(i, j, color); | |
| } | |
| } | |
| updateBoundingBox(x, y, x+w, y+h); | |
| } | |
| // draw a rectangle | |
| void ST7565::drawrect(unsigned char x, unsigned char y, unsigned char w, unsigned char h, | |
| unsigned char color) { | |
| // stupidest version - just pixels - but fast with internal buffer! | |
| for (unsigned char i=x; i<x+w; i++) { | |
| my_setpixel(i, y, color); | |
| my_setpixel(i, y+h-1, color); | |
| } | |
| for (unsigned char i=y; i<y+h; i++) { | |
| my_setpixel(x, i, color); | |
| my_setpixel(x+w-1, i, color); | |
| } | |
| updateBoundingBox(x, y, x+w, y+h); | |
| } | |
| // draw a circle outline | |
| void ST7565::drawcircle(unsigned char x0, unsigned char y0, unsigned char r, | |
| unsigned char color) { | |
| updateBoundingBox(x0-r, y0-r, x0+r, y0+r); | |
| int8_t f = 1 - r; | |
| int8_t ddF_x = 1; | |
| int8_t ddF_y = -2 * r; | |
| int8_t x = 0; | |
| int8_t y = r; | |
| my_setpixel(x0, y0+r, color); | |
| my_setpixel(x0, y0-r, color); | |
| my_setpixel(x0+r, y0, color); | |
| my_setpixel(x0-r, y0, color); | |
| while (x<y) { | |
| if (f >= 0) { | |
| y--; | |
| ddF_y += 2; | |
| f += ddF_y; | |
| } | |
| x++; | |
| ddF_x += 2; | |
| f += ddF_x; | |
| my_setpixel(x0 + x, y0 + y, color); | |
| my_setpixel(x0 - x, y0 + y, color); | |
| my_setpixel(x0 + x, y0 - y, color); | |
| my_setpixel(x0 - x, y0 - y, color); | |
| my_setpixel(x0 + y, y0 + x, color); | |
| my_setpixel(x0 - y, y0 + x, color); | |
| my_setpixel(x0 + y, y0 - x, color); | |
| my_setpixel(x0 - y, y0 - x, color); | |
| } | |
| } | |
| void ST7565::fillcircle(unsigned char x0, unsigned char y0, unsigned char r, | |
| unsigned char color) { | |
| updateBoundingBox(x0-r, y0-r, x0+r, y0+r); | |
| int8_t f = 1 - r; | |
| int8_t ddF_x = 1; | |
| int8_t ddF_y = -2 * r; | |
| int8_t x = 0; | |
| int8_t y = r; | |
| for (unsigned char i=y0-r; i<=y0+r; i++) { | |
| my_setpixel(x0, i, color); | |
| } | |
| while (x<y) { | |
| if (f >= 0) { | |
| y--; | |
| ddF_y += 2; | |
| f += ddF_y; | |
| } | |
| x++; | |
| ddF_x += 2; | |
| f += ddF_x; | |
| for (unsigned char i=y0-y; i<=y0+y; i++) { | |
| my_setpixel(x0+x, i, color); | |
| my_setpixel(x0-x, i, color); | |
| } | |
| for (unsigned char i=y0-x; i<=y0+x; i++) { | |
| my_setpixel(x0+y, i, color); | |
| my_setpixel(x0-y, i, color); | |
| } | |
| } | |
| } | |
| void ST7565::my_setpixel(unsigned char x, unsigned char y, unsigned char color) { | |
| if ((x >= LCDWIDTH) || (y >= LCDHEIGHT)) | |
| return; | |
| // x is which column | |
| if (color) | |
| st7565_buffer[x+ (y/8)*128] |= _BV(7-(y%8)); | |
| else | |
| st7565_buffer[x+ (y/8)*128] &= ~_BV(7-(y%8)); | |
| } | |
| // the most basic function, set a single pixel | |
| void ST7565::setpixel(unsigned char x, unsigned char y, unsigned char color) { | |
| if ((x >= LCDWIDTH) || (y >= LCDHEIGHT)) | |
| return; | |
| // x is which column | |
| if (color) | |
| st7565_buffer[x+ (y/8)*128] |= _BV(7-(y%8)); | |
| else | |
| st7565_buffer[x+ (y/8)*128] &= ~_BV(7-(y%8)); | |
| updateBoundingBox(x,y,x,y); | |
| } | |
| // the most basic function, get a single pixel | |
| unsigned char ST7565::getpixel(unsigned char x, unsigned char y) { | |
| if ((x >= LCDWIDTH) || (y >= LCDHEIGHT)) | |
| return 0; | |
| return (st7565_buffer[x+ (y/8)*128] >> (7-(y%8))) & 0x1; | |
| } | |
| void ST7565::begin(unsigned char contrast) { | |
| st7565_init(); | |
| st7565_command(CMD_DISPLAY_ON); | |
| st7565_command(CMD_SET_ALLPTS_NORMAL); | |
| st7565_set_brightness(contrast); | |
| } | |
| void ST7565::st7565_init(void) { | |
| // set pin directions | |
| pinMode(sid, OUTPUT); | |
| pinMode(sclk, OUTPUT); | |
| pinMode(a0, OUTPUT); | |
| pinMode(rst, OUTPUT); | |
| pinMode(cs, OUTPUT); | |
| // toggle RST low to reset; CS low so it'll listen to us | |
| if (cs > 0) | |
| digitalWrite(cs, LOW); | |
| digitalWrite(rst, LOW); | |
| delay(500); | |
| digitalWrite(rst, HIGH); | |
| // LCD bias select | |
| st7565_command(CMD_SET_BIAS_7); | |
| // ADC select | |
| st7565_command(CMD_SET_ADC_NORMAL); | |
| // SHL select | |
| st7565_command(CMD_SET_COM_NORMAL); | |
| // Initial display line | |
| st7565_command(CMD_SET_DISP_START_LINE); | |
| // turn on voltage converter (VC=1, VR=0, VF=0) | |
| st7565_command(CMD_SET_POWER_CONTROL | 0x4); | |
| // wait for 50% rising | |
| delay(50); | |
| // turn on voltage regulator (VC=1, VR=1, VF=0) | |
| st7565_command(CMD_SET_POWER_CONTROL | 0x6); | |
| // wait >=50ms | |
| delay(50); | |
| // turn on voltage follower (VC=1, VR=1, VF=1) | |
| st7565_command(CMD_SET_POWER_CONTROL | 0x7); | |
| // wait | |
| delay(10); | |
| // set lcd operating voltage (regulator resistor, ref voltage resistor) | |
| st7565_command(CMD_SET_RESISTOR_RATIO | 0x6); | |
| // initial display line | |
| // set page address | |
| // set column address | |
| // write display data | |
| // set up a bounding box for screen updates | |
| updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1); | |
| } | |
| inline void ST7565::spiwrite(unsigned char c) { | |
| shiftOut(sid, sclk, MSBFIRST, c); | |
| /* | |
| int8_t i; | |
| for (i=7; i>=0; i--) { | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(i)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| } | |
| */ | |
| /* | |
| // loop unwrapped! too fast doesnt work :( | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(7)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(6)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(5)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(4)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(3)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(2)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(1)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| SCLK_PORT &= ~_BV(SCLK); | |
| if (c & _BV(0)) | |
| SID_PORT |= _BV(SID); | |
| else | |
| SID_PORT &= ~_BV(SID); | |
| SCLK_PORT |= _BV(SCLK); | |
| */ | |
| } | |
| void ST7565::st7565_command(unsigned char c) { | |
| digitalWrite(a0, LOW); | |
| spiwrite(c); | |
| } | |
| void ST7565::st7565_data(unsigned char c) { | |
| digitalWrite(a0, HIGH); | |
| spiwrite(c); | |
| } | |
| void ST7565::st7565_set_brightness(unsigned char val) { | |
| st7565_command(0x23); // Vo voltage resistor ratio set | |
| st7565_command(CMD_SET_VOLUME_FIRST); | |
| st7565_command(CMD_SET_VOLUME_SECOND | (val & 0x3f)); | |
| } | |
| void ST7565::display(void) { | |
| unsigned char col, maxcol, p; | |
| /* | |
| Serial.print("Refresh ("); Serial.print(xUpdateMin, DEC); | |
| Serial.print(", "); Serial.print(xUpdateMax, DEC); | |
| Serial.print(","); Serial.print(yUpdateMin, DEC); | |
| Serial.print(", "); Serial.print(yUpdateMax, DEC); Serial.println(")"); | |
| */ | |
| for(p = 0; p < 8; p++) { | |
| /* | |
| putstring("new page! "); | |
| uart_putw_dec(p); | |
| putstring_nl(""); | |
| */ | |
| #ifdef enablePartialUpdate | |
| // check if this page is part of update | |
| if ( yUpdateMin >= ((p+1)*8) ) { | |
| continue; // nope, skip it! | |
| } | |
| if (yUpdateMax < p*8) { | |
| break; | |
| } | |
| #endif | |
| st7565_command(CMD_SET_PAGE | pagemap[p]); | |
| #ifdef enablePartialUpdate | |
| col = xUpdateMin; | |
| maxcol = xUpdateMax; | |
| #else | |
| // start at the beginning of the row | |
| col = 0; | |
| maxcol = LCDWIDTH-1; | |
| #endif | |
| st7565_command(CMD_SET_COLUMN_LOWER | ((col+ST7565_STARTBYTES) & 0xf)); | |
| st7565_command(CMD_SET_COLUMN_UPPER | (((col+ST7565_STARTBYTES) >> 4) & 0x0F)); | |
| st7565_command(CMD_RMW); | |
| for(; col <= maxcol; col++) { | |
| //uart_putw_dec(col); | |
| //uart_putchar(' '); | |
| st7565_data(st7565_buffer[(128*p)+col]); | |
| } | |
| } | |
| #ifdef enablePartialUpdate | |
| xUpdateMin = LCDWIDTH - 1; | |
| xUpdateMax = 0; | |
| yUpdateMin = LCDHEIGHT-1; | |
| yUpdateMax = 0; | |
| #endif | |
| } | |
| // clear everything | |
| void ST7565::clear(void) { | |
| memset(st7565_buffer, 0, 1024); | |
| updateBoundingBox(0, 0, LCDWIDTH-1, LCDHEIGHT-1); | |
| } | |
| // this doesnt touch the buffer, just clears the display RAM - might be handy | |
| void ST7565::clear_display(void) { | |
| unsigned char p, c; | |
| for(p = 0; p < 8; p++) { | |
| /* | |
| putstring("new page! "); | |
| uart_putw_dec(p); | |
| putstring_nl(""); | |
| */ | |
| st7565_command(CMD_SET_PAGE | p); | |
| for(c = 0; c < 129; c++) { | |
| //uart_putw_dec(c); | |
| //uart_putchar(' '); | |
| st7565_command(CMD_SET_COLUMN_LOWER | (c & 0xf)); | |
| st7565_command(CMD_SET_COLUMN_UPPER | ((c >> 4) & 0xf)); | |
| st7565_data(0x0); | |
| } | |
| } | |
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| /* | |
| $Id:$ | |
| ST7565 LCD library! | |
| Copyright (C) 2010 Limor Fried, Adafruit Industries | |
| This library is free software; you can redistribute it and/or | |
| modify it under the terms of the GNU Lesser General Public | |
| License as published by the Free Software Foundation; either | |
| version 2.1 of the License, or (at your option) any later version. | |
| This library is distributed in the hope that it will be useful, | |
| but WITHOUT ANY WARRANTY; without even the implied warranty of | |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | |
| Lesser General Public License for more details. | |
| You should have received a copy of the GNU Lesser General Public | |
| License along with this library; if not, write to the Free Software | |
| Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA | |
| // some of this code was written by <cstone@pobox.com> originally; it is in the public domain. | |
| // this source is modified for AQM1284 by kkurahahsi | |
| */ | |
| #include "Arduino.h" | |
| #define swap(a, b) { unsigned char t = a; a = b; b = t; } | |
| #define BLACK 1 | |
| #define WHITE 0 | |
| #define LCDWIDTH 128 | |
| #define LCDHEIGHT 48 | |
| #define CMD_DISPLAY_OFF 0xAE | |
| #define CMD_DISPLAY_ON 0xAF | |
| #define CMD_SET_DISP_START_LINE 0x40 | |
| #define CMD_SET_PAGE 0xB0 | |
| #define CMD_SET_COLUMN_UPPER 0x10 | |
| #define CMD_SET_COLUMN_LOWER 0x00 | |
| #define CMD_SET_ADC_NORMAL 0xA0 | |
| #define CMD_SET_ADC_REVERSE 0xA1 | |
| #define CMD_SET_DISP_NORMAL 0xA6 | |
| #define CMD_SET_DISP_REVERSE 0xA7 | |
| #define CMD_SET_ALLPTS_NORMAL 0xA4 | |
| #define CMD_SET_ALLPTS_ON 0xA5 | |
| #define CMD_SET_BIAS_9 0xA2 | |
| #define CMD_SET_BIAS_7 0xA3 | |
| #define CMD_RMW 0xE0 | |
| #define CMD_RMW_CLEAR 0xEE | |
| #define CMD_INTERNAL_RESET 0xE2 | |
| #define CMD_SET_COM_NORMAL 0xC0 | |
| #define CMD_SET_COM_REVERSE 0xC8 | |
| #define CMD_SET_POWER_CONTROL 0x28 | |
| #define CMD_SET_RESISTOR_RATIO 0x20 | |
| #define CMD_SET_VOLUME_FIRST 0x81 | |
| #define CMD_SET_VOLUME_SECOND 0 | |
| #define CMD_SET_STATIC_OFF 0xAC | |
| #define CMD_SET_STATIC_ON 0xAD | |
| #define CMD_SET_STATIC_REG 0x0 | |
| #define CMD_SET_BOOSTER_FIRST 0xF8 | |
| #define CMD_SET_BOOSTER_234 0 | |
| #define CMD_SET_BOOSTER_5 1 | |
| #define CMD_SET_BOOSTER_6 3 | |
| #define CMD_NOP 0xE3 | |
| #define CMD_TEST 0xF0 | |
| class ST7565 { | |
| public: | |
| ST7565(int8_t SID, int8_t SCLK, int8_t A0, int8_t RST, int8_t CS) :sid(SID), sclk(SCLK), a0(A0), rst(RST), cs(CS) {} | |
| ST7565(int8_t SID, int8_t SCLK, int8_t A0, int8_t RST) :sid(SID), sclk(SCLK), a0(A0), rst(RST), cs(-1) {} | |
| void st7565_init(void); | |
| void begin(unsigned char contrast); | |
| void st7565_command(unsigned char c); | |
| void st7565_data(unsigned char c); | |
| void st7565_set_brightness(unsigned char val); | |
| void clear_display(void); | |
| void clear(); | |
| void display(); | |
| void setpixel(unsigned char x, unsigned char y, unsigned char color); | |
| unsigned char getpixel(unsigned char x, unsigned char y); | |
| void fillcircle(unsigned char x0, unsigned char y0, unsigned char r, | |
| unsigned char color); | |
| void drawcircle(unsigned char x0, unsigned char y0, unsigned char r, | |
| unsigned char color); | |
| void drawrect(unsigned char x, unsigned char y, unsigned char w, unsigned char h, | |
| unsigned char color); | |
| void fillrect(unsigned char x, unsigned char y, unsigned char w, unsigned char h, | |
| unsigned char color); | |
| void drawline(unsigned char x0, unsigned char y0, unsigned char x1, unsigned char y1, | |
| unsigned char color); | |
| void drawchar(unsigned char x, unsigned char line, char c); | |
| void drawstring(unsigned char x, unsigned char line, char *c); | |
| void drawstring_P(unsigned char x, unsigned char line, const char *c); | |
| void drawbitmap(unsigned char x, unsigned char y, | |
| const unsigned char *bitmap, unsigned char w, unsigned char h, | |
| unsigned char color); | |
| private: | |
| int8_t sid, sclk, a0, rst, cs; | |
| void spiwrite(unsigned char c); | |
| void my_setpixel(unsigned char x, unsigned char y, unsigned char color); | |
| //unsigned char buffer[128*64/8]; | |
| }; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <ST7565.h> | |
| #define NMAX 256 | |
| #define NMAX_HALF (NMAX/2) | |
| #define NMAXSQRT 16 | |
| #define MAX(x,y) ((x) > (y) ? (x) : (y)) | |
| // fft buffer / workarea | |
| float micBuffer[NMAX+1]; | |
| float w[NMAX * 5 / 4+1]; | |
| int ip[NMAXSQRT + 2]; | |
| void rdft(int n, int isgn, float *a, int *ip, float *w); | |
| #define WAIT_FOR_SAMPLING (1000/32) // 32khz sampling | |
| // Microphone ADC port | |
| #define MIC 13 | |
| // LCD | |
| byte LCDBuffer[LCDWIDTH]; | |
| int FFTCounter; | |
| // Generate Test data | |
| void putdata(int n, float *a) | |
| { | |
| int j; | |
| float pi2 = 3.14159265*2 / n; | |
| for (j = 0; j <n; j++) { | |
| a[j] = sin(j*pi2*10.0)*10.0 + sin(j*pi2*15.0)*5.0 + sin(j*pi2*20.0)*10.0 + 25.0; | |
| } | |
| } | |
| void setup() | |
| { | |
| Serial.begin(115200); | |
| FFTCounter = 0; | |
| ip[0] = 0; | |
| } | |
| void loop() | |
| { | |
| // sprint buffer | |
| char dispbuf[20]; | |
| // time measurement | |
| unsigned long t = millis(); | |
| // sampling from microphone | |
| for (int i = 0; i < NMAX; i++) { | |
| int a = analogRead(MIC) - 400; | |
| micBuffer[i] = a; | |
| delayMicroseconds(WAIT_FOR_SAMPLING); | |
| } | |
| // putdata(NMAX, micBuffer); | |
| unsigned long d = millis() - t; | |
| Serial.print("read = "); | |
| Serial.print(d); | |
| // FFT | |
| t = millis(); | |
| // ip[0] = 0; | |
| rdft(NMAX, 1, micBuffer, ip, w); | |
| // rdft(n, -1, micBuffer, ip, w); | |
| FFTCounter ++; | |
| if (FFTCounter > 9999) FFTCounter = 0; | |
| d = millis() - t; | |
| Serial.print(", calc = "); | |
| Serial.print(d); | |
| // calc maximum value | |
| t = millis(); | |
| float max = 0; | |
| for (int i = 0; i < NMAX_HALF; i++) { | |
| int i2 = i*2; | |
| micBuffer[i] = micBuffer[i2]*micBuffer[i2] + micBuffer[i2+1]*micBuffer[i2+1]; | |
| if (i > 0 && micBuffer[i] > max) max = micBuffer[i]; | |
| } | |
| // set display data | |
| max = LCDHEIGHT / max; | |
| for (int i = 1; i < NMAX_HALF; i++) { | |
| int y = LCDHEIGHT-max*micBuffer[i]; | |
| if (y < 0) y = 0; | |
| LCDBuffer[i] = y; | |
| } | |
| Serial.println(); | |
| } | |
| /* | |
| Fast Fourier/Cosine/Sine Transform | |
| dimension :one | |
| data length :power of 2 | |
| decimation :frequency | |
| radix :4, 2 | |
| data :inplace | |
| table :use | |
| functions | |
| cdft: Complex Discrete Fourier Transform | |
| rdft: Real Discrete Fourier Transform | |
| ddct: Discrete Cosine Transform | |
| ddst: Discrete Sine Transform | |
| dfct: Cosine Transform of RDFT (Real Symmetric DFT) | |
| dfst: Sine Transform of RDFT (Real Anti-symmetric DFT) | |
| function prototypes | |
| void cdft(int, int, float *, int *, float *); | |
| void rdft(int, int, float *, int *, float *); | |
| void ddct(int, int, float *, int *, float *); | |
| void ddst(int, int, float *, int *, float *); | |
| void dfct(int, float *, float *, int *, float *); | |
| void dfst(int, float *, float *, int *, float *); | |
| -------- Complex DFT (Discrete Fourier Transform) -------- | |
| [definition] | |
| <case1> | |
| X[k] = sum_j=0^n-1 x[j]*exp(2*pi*i*j*k/n), 0<=k<n | |
| <case2> | |
| X[k] = sum_j=0^n-1 x[j]*exp(-2*pi*i*j*k/n), 0<=k<n | |
| (notes: sum_j=0^n-1 is a summation from j=0 to n-1) | |
| [usage] | |
| <case1> | |
| ip[0] = 0; // first time only | |
| cdft(2*n, 1, a, ip, w); | |
| <case2> | |
| ip[0] = 0; // first time only | |
| cdft(2*n, -1, a, ip, w); | |
| [parameters] | |
| 2*n :data length (int) | |
| n >= 1, n = power of 2 | |
| a[0...2*n-1] :input/output data (float *) | |
| input data | |
| a[2*j] = Re(x[j]), | |
| a[2*j+1] = Im(x[j]), 0<=j<n | |
| output data | |
| a[2*k] = Re(X[k]), | |
| a[2*k+1] = Im(X[k]), 0<=k<n | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n/2-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| cdft(2*n, -1, a, ip, w); | |
| is | |
| cdft(2*n, 1, a, ip, w); | |
| for (j = 0; j <= 2 * n - 1; j++) { | |
| a[j] *= 1.0 / n; | |
| } | |
| . | |
| -------- Real DFT / Inverse of Real DFT -------- | |
| [definition] | |
| <case1> RDFT | |
| R[k] = sum_j=0^n-1 a[j]*cos(2*pi*j*k/n), 0<=k<=n/2 | |
| I[k] = sum_j=0^n-1 a[j]*sin(2*pi*j*k/n), 0<k<n/2 | |
| <case2> IRDFT (excluding scale) | |
| a[k] = (R[0] + R[n/2]*cos(pi*k))/2 + | |
| sum_j=1^n/2-1 R[j]*cos(2*pi*j*k/n) + | |
| sum_j=1^n/2-1 I[j]*sin(2*pi*j*k/n), 0<=k<n | |
| [usage] | |
| <case1> | |
| ip[0] = 0; // first time only | |
| rdft(n, 1, a, ip, w); | |
| <case2> | |
| ip[0] = 0; // first time only | |
| rdft(n, -1, a, ip, w); | |
| [parameters] | |
| n :data length (int) | |
| n >= 2, n = power of 2 | |
| a[0...n-1] :input/output data (float *) | |
| <case1> | |
| output data | |
| a[2*k] = R[k], 0<=k<n/2 | |
| a[2*k+1] = I[k], 0<k<n/2 | |
| a[1] = R[n/2] | |
| <case2> | |
| input data | |
| a[2*j] = R[j], 0<=j<n/2 | |
| a[2*j+1] = I[j], 0<j<n/2 | |
| a[1] = R[n/2] | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n/2) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n/2+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n/2-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| rdft(n, 1, a, ip, w); | |
| is | |
| rdft(n, -1, a, ip, w); | |
| for (j = 0; j <= n - 1; j++) { | |
| a[j] *= 2.0 / n; | |
| } | |
| . | |
| -------- DCT (Discrete Cosine Transform) / Inverse of DCT -------- | |
| [definition] | |
| <case1> IDCT (excluding scale) | |
| C[k] = sum_j=0^n-1 a[j]*cos(pi*j*(k+1/2)/n), 0<=k<n | |
| <case2> DCT | |
| C[k] = sum_j=0^n-1 a[j]*cos(pi*(j+1/2)*k/n), 0<=k<n | |
| [usage] | |
| <case1> | |
| ip[0] = 0; // first time only | |
| ddct(n, 1, a, ip, w); | |
| <case2> | |
| ip[0] = 0; // first time only | |
| ddct(n, -1, a, ip, w); | |
| [parameters] | |
| n :data length (int) | |
| n >= 2, n = power of 2 | |
| a[0...n-1] :input/output data (float *) | |
| output data | |
| a[k] = C[k], 0<=k<n | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n/2) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n/2+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n*5/4-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| ddct(n, -1, a, ip, w); | |
| is | |
| a[0] *= 0.5; | |
| ddct(n, 1, a, ip, w); | |
| for (j = 0; j <= n - 1; j++) { | |
| a[j] *= 2.0 / n; | |
| } | |
| . | |
| -------- DST (Discrete Sine Transform) / Inverse of DST -------- | |
| [definition] | |
| <case1> IDST (excluding scale) | |
| S[k] = sum_j=1^n A[j]*sin(pi*j*(k+1/2)/n), 0<=k<n | |
| <case2> DST | |
| S[k] = sum_j=0^n-1 a[j]*sin(pi*(j+1/2)*k/n), 0<k<=n | |
| [usage] | |
| <case1> | |
| ip[0] = 0; // first time only | |
| ddst(n, 1, a, ip, w); | |
| <case2> | |
| ip[0] = 0; // first time only | |
| ddst(n, -1, a, ip, w); | |
| [parameters] | |
| n :data length (int) | |
| n >= 2, n = power of 2 | |
| a[0...n-1] :input/output data (float *) | |
| <case1> | |
| input data | |
| a[j] = A[j], 0<j<n | |
| a[0] = A[n] | |
| output data | |
| a[k] = S[k], 0<=k<n | |
| <case2> | |
| output data | |
| a[k] = S[k], 0<k<n | |
| a[0] = S[n] | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n/2) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n/2+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n*5/4-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| ddst(n, -1, a, ip, w); | |
| is | |
| a[0] *= 0.5; | |
| ddst(n, 1, a, ip, w); | |
| for (j = 0; j <= n - 1; j++) { | |
| a[j] *= 2.0 / n; | |
| } | |
| . | |
| -------- Cosine Transform of RDFT (Real Symmetric DFT) -------- | |
| [definition] | |
| C[k] = sum_j=0^n a[j]*cos(pi*j*k/n), 0<=k<=n | |
| [usage] | |
| ip[0] = 0; // first time only | |
| dfct(n, a, t, ip, w); | |
| [parameters] | |
| n :data length - 1 (int) | |
| n >= 2, n = power of 2 | |
| a[0...n] :input/output data (float *) | |
| output data | |
| a[k] = C[k], 0<=k<=n | |
| t[0...n/2] :work area (float *) | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n/4) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n/4+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n*5/8-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| a[0] *= 0.5; | |
| a[n] *= 0.5; | |
| dfct(n, a, t, ip, w); | |
| is | |
| a[0] *= 0.5; | |
| a[n] *= 0.5; | |
| dfct(n, a, t, ip, w); | |
| for (j = 0; j <= n; j++) { | |
| a[j] *= 2.0 / n; | |
| } | |
| . | |
| -------- Sine Transform of RDFT (Real Anti-symmetric DFT) -------- | |
| [definition] | |
| S[k] = sum_j=1^n-1 a[j]*sin(pi*j*k/n), 0<k<n | |
| [usage] | |
| ip[0] = 0; // first time only | |
| dfst(n, a, t, ip, w); | |
| [parameters] | |
| n :data length + 1 (int) | |
| n >= 2, n = power of 2 | |
| a[0...n-1] :input/output data (float *) | |
| output data | |
| a[k] = S[k], 0<k<n | |
| (a[0] is used for work area) | |
| t[0...n/2-1] :work area (float *) | |
| ip[0...*] :work area for bit reversal (int *) | |
| length of ip >= 2+sqrt(n/4) | |
| strictly, | |
| length of ip >= | |
| 2+(1<<(int)(log(n/4+0.5)/log(2))/2). | |
| ip[0],ip[1] are pointers of the cos/sin table. | |
| w[0...n*5/8-1] :cos/sin table (float *) | |
| w[],ip[] are initialized if ip[0] == 0. | |
| [remark] | |
| Inverse of | |
| dfst(n, a, t, ip, w); | |
| is | |
| dfst(n, a, t, ip, w); | |
| for (j = 1; j <= n - 1; j++) { | |
| a[j] *= 2.0 / n; | |
| } | |
| . | |
| Appendix : | |
| The cos/sin table is recalculated when the larger table required. | |
| w[] and ip[] are compatible with all routines. | |
| */ | |
| void cdft(int n, int isgn, float *a, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void bitrv2(int n, int *ip, float *a); | |
| void bitrv2conj(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void cftbsub(int n, float *a, float *w); | |
| if (n > (ip[0] << 2)) { | |
| makewt(n >> 2, ip, w); | |
| } | |
| if (n > 4) { | |
| if (isgn >= 0) { | |
| bitrv2(n, ip + 2, a); | |
| cftfsub(n, a, w); | |
| } | |
| else { | |
| bitrv2conj(n, ip + 2, a); | |
| cftbsub(n, a, w); | |
| } | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| } | |
| void rdft(int n, int isgn, float *a, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void makect(int nc, int *ip, float *c); | |
| void bitrv2(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void cftbsub(int n, float *a, float *w); | |
| void rftfsub(int n, float *a, int nc, float *c); | |
| void rftbsub(int n, float *a, int nc, float *c); | |
| int nw, nc; | |
| float xi; | |
| nw = ip[0]; | |
| if (n > (nw << 2)) { | |
| nw = n >> 2; | |
| makewt(nw, ip, w); | |
| } | |
| nc = ip[1]; | |
| if (n > (nc << 2)) { | |
| nc = n >> 2; | |
| makect(nc, ip, w + nw); | |
| } | |
| if (isgn >= 0) { | |
| if (n > 4) { | |
| bitrv2(n, ip + 2, a); | |
| cftfsub(n, a, w); | |
| rftfsub(n, a, nc, w + nw); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| xi = a[0] - a[1]; | |
| a[0] += a[1]; | |
| a[1] = xi; | |
| } | |
| else { | |
| a[1] = 0.5 * (a[0] - a[1]); | |
| a[0] -= a[1]; | |
| if (n > 4) { | |
| rftbsub(n, a, nc, w + nw); | |
| bitrv2(n, ip + 2, a); | |
| cftbsub(n, a, w); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| } | |
| } | |
| void ddct(int n, int isgn, float *a, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void makect(int nc, int *ip, float *c); | |
| void bitrv2(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void cftbsub(int n, float *a, float *w); | |
| void rftfsub(int n, float *a, int nc, float *c); | |
| void rftbsub(int n, float *a, int nc, float *c); | |
| void dctsub(int n, float *a, int nc, float *c); | |
| int j, nw, nc; | |
| float xr; | |
| nw = ip[0]; | |
| if (n > (nw << 2)) { | |
| nw = n >> 2; | |
| makewt(nw, ip, w); | |
| } | |
| nc = ip[1]; | |
| if (n > nc) { | |
| nc = n; | |
| makect(nc, ip, w + nw); | |
| } | |
| if (isgn < 0) { | |
| xr = a[n - 1]; | |
| for (j = n - 2; j >= 2; j -= 2) { | |
| a[j + 1] = a[j] - a[j - 1]; | |
| a[j] += a[j - 1]; | |
| } | |
| a[1] = a[0] - xr; | |
| a[0] += xr; | |
| if (n > 4) { | |
| rftbsub(n, a, nc, w + nw); | |
| bitrv2(n, ip + 2, a); | |
| cftbsub(n, a, w); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| } | |
| dctsub(n, a, nc, w + nw); | |
| if (isgn >= 0) { | |
| if (n > 4) { | |
| bitrv2(n, ip + 2, a); | |
| cftfsub(n, a, w); | |
| rftfsub(n, a, nc, w + nw); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| xr = a[0] - a[1]; | |
| a[0] += a[1]; | |
| for (j = 2; j < n; j += 2) { | |
| a[j - 1] = a[j] - a[j + 1]; | |
| a[j] += a[j + 1]; | |
| } | |
| a[n - 1] = xr; | |
| } | |
| } | |
| void ddst(int n, int isgn, float *a, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void makect(int nc, int *ip, float *c); | |
| void bitrv2(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void cftbsub(int n, float *a, float *w); | |
| void rftfsub(int n, float *a, int nc, float *c); | |
| void rftbsub(int n, float *a, int nc, float *c); | |
| void dstsub(int n, float *a, int nc, float *c); | |
| int j, nw, nc; | |
| float xr; | |
| nw = ip[0]; | |
| if (n > (nw << 2)) { | |
| nw = n >> 2; | |
| makewt(nw, ip, w); | |
| } | |
| nc = ip[1]; | |
| if (n > nc) { | |
| nc = n; | |
| makect(nc, ip, w + nw); | |
| } | |
| if (isgn < 0) { | |
| xr = a[n - 1]; | |
| for (j = n - 2; j >= 2; j -= 2) { | |
| a[j + 1] = -a[j] - a[j - 1]; | |
| a[j] -= a[j - 1]; | |
| } | |
| a[1] = a[0] + xr; | |
| a[0] -= xr; | |
| if (n > 4) { | |
| rftbsub(n, a, nc, w + nw); | |
| bitrv2(n, ip + 2, a); | |
| cftbsub(n, a, w); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| } | |
| dstsub(n, a, nc, w + nw); | |
| if (isgn >= 0) { | |
| if (n > 4) { | |
| bitrv2(n, ip + 2, a); | |
| cftfsub(n, a, w); | |
| rftfsub(n, a, nc, w + nw); | |
| } | |
| else if (n == 4) { | |
| cftfsub(n, a, w); | |
| } | |
| xr = a[0] - a[1]; | |
| a[0] += a[1]; | |
| for (j = 2; j < n; j += 2) { | |
| a[j - 1] = -a[j] - a[j + 1]; | |
| a[j] -= a[j + 1]; | |
| } | |
| a[n - 1] = -xr; | |
| } | |
| } | |
| void dfct(int n, float *a, float *t, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void makect(int nc, int *ip, float *c); | |
| void bitrv2(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void rftfsub(int n, float *a, int nc, float *c); | |
| void dctsub(int n, float *a, int nc, float *c); | |
| int j, k, l, m, mh, nw, nc; | |
| float xr, xi, yr, yi; | |
| nw = ip[0]; | |
| if (n > (nw << 3)) { | |
| nw = n >> 3; | |
| makewt(nw, ip, w); | |
| } | |
| nc = ip[1]; | |
| if (n > (nc << 1)) { | |
| nc = n >> 1; | |
| makect(nc, ip, w + nw); | |
| } | |
| m = n >> 1; | |
| yi = a[m]; | |
| xi = a[0] + a[n]; | |
| a[0] -= a[n]; | |
| t[0] = xi - yi; | |
| t[m] = xi + yi; | |
| if (n > 2) { | |
| mh = m >> 1; | |
| for (j = 1; j < mh; j++) { | |
| k = m - j; | |
| xr = a[j] - a[n - j]; | |
| xi = a[j] + a[n - j]; | |
| yr = a[k] - a[n - k]; | |
| yi = a[k] + a[n - k]; | |
| a[j] = xr; | |
| a[k] = yr; | |
| t[j] = xi - yi; | |
| t[k] = xi + yi; | |
| } | |
| t[mh] = a[mh] + a[n - mh]; | |
| a[mh] -= a[n - mh]; | |
| dctsub(m, a, nc, w + nw); | |
| if (m > 4) { | |
| bitrv2(m, ip + 2, a); | |
| cftfsub(m, a, w); | |
| rftfsub(m, a, nc, w + nw); | |
| } | |
| else if (m == 4) { | |
| cftfsub(m, a, w); | |
| } | |
| a[n - 1] = a[0] - a[1]; | |
| a[1] = a[0] + a[1]; | |
| for (j = m - 2; j >= 2; j -= 2) { | |
| a[2 * j + 1] = a[j] + a[j + 1]; | |
| a[2 * j - 1] = a[j] - a[j + 1]; | |
| } | |
| l = 2; | |
| m = mh; | |
| while (m >= 2) { | |
| dctsub(m, t, nc, w + nw); | |
| if (m > 4) { | |
| bitrv2(m, ip + 2, t); | |
| cftfsub(m, t, w); | |
| rftfsub(m, t, nc, w + nw); | |
| } | |
| else if (m == 4) { | |
| cftfsub(m, t, w); | |
| } | |
| a[n - l] = t[0] - t[1]; | |
| a[l] = t[0] + t[1]; | |
| k = 0; | |
| for (j = 2; j < m; j += 2) { | |
| k += l << 2; | |
| a[k - l] = t[j] - t[j + 1]; | |
| a[k + l] = t[j] + t[j + 1]; | |
| } | |
| l <<= 1; | |
| mh = m >> 1; | |
| for (j = 0; j < mh; j++) { | |
| k = m - j; | |
| t[j] = t[m + k] - t[m + j]; | |
| t[k] = t[m + k] + t[m + j]; | |
| } | |
| t[mh] = t[m + mh]; | |
| m = mh; | |
| } | |
| a[l] = t[0]; | |
| a[n] = t[2] - t[1]; | |
| a[0] = t[2] + t[1]; | |
| } | |
| else { | |
| a[1] = a[0]; | |
| a[2] = t[0]; | |
| a[0] = t[1]; | |
| } | |
| } | |
| void dfst(int n, float *a, float *t, int *ip, float *w) | |
| { | |
| void makewt(int nw, int *ip, float *w); | |
| void makect(int nc, int *ip, float *c); | |
| void bitrv2(int n, int *ip, float *a); | |
| void cftfsub(int n, float *a, float *w); | |
| void rftfsub(int n, float *a, int nc, float *c); | |
| void dstsub(int n, float *a, int nc, float *c); | |
| int j, k, l, m, mh, nw, nc; | |
| float xr, xi, yr, yi; | |
| nw = ip[0]; | |
| if (n > (nw << 3)) { | |
| nw = n >> 3; | |
| makewt(nw, ip, w); | |
| } | |
| nc = ip[1]; | |
| if (n > (nc << 1)) { | |
| nc = n >> 1; | |
| makect(nc, ip, w + nw); | |
| } | |
| if (n > 2) { | |
| m = n >> 1; | |
| mh = m >> 1; | |
| for (j = 1; j < mh; j++) { | |
| k = m - j; | |
| xr = a[j] + a[n - j]; | |
| xi = a[j] - a[n - j]; | |
| yr = a[k] + a[n - k]; | |
| yi = a[k] - a[n - k]; | |
| a[j] = xr; | |
| a[k] = yr; | |
| t[j] = xi + yi; | |
| t[k] = xi - yi; | |
| } | |
| t[0] = a[mh] - a[n - mh]; | |
| a[mh] += a[n - mh]; | |
| a[0] = a[m]; | |
| dstsub(m, a, nc, w + nw); | |
| if (m > 4) { | |
| bitrv2(m, ip + 2, a); | |
| cftfsub(m, a, w); | |
| rftfsub(m, a, nc, w + nw); | |
| } | |
| else if (m == 4) { | |
| cftfsub(m, a, w); | |
| } | |
| a[n - 1] = a[1] - a[0]; | |
| a[1] = a[0] + a[1]; | |
| for (j = m - 2; j >= 2; j -= 2) { | |
| a[2 * j + 1] = a[j] - a[j + 1]; | |
| a[2 * j - 1] = -a[j] - a[j + 1]; | |
| } | |
| l = 2; | |
| m = mh; | |
| while (m >= 2) { | |
| dstsub(m, t, nc, w + nw); | |
| if (m > 4) { | |
| bitrv2(m, ip + 2, t); | |
| cftfsub(m, t, w); | |
| rftfsub(m, t, nc, w + nw); | |
| } | |
| else if (m == 4) { | |
| cftfsub(m, t, w); | |
| } | |
| a[n - l] = t[1] - t[0]; | |
| a[l] = t[0] + t[1]; | |
| k = 0; | |
| for (j = 2; j < m; j += 2) { | |
| k += l << 2; | |
| a[k - l] = -t[j] - t[j + 1]; | |
| a[k + l] = t[j] - t[j + 1]; | |
| } | |
| l <<= 1; | |
| mh = m >> 1; | |
| for (j = 1; j < mh; j++) { | |
| k = m - j; | |
| t[j] = t[m + k] + t[m + j]; | |
| t[k] = t[m + k] - t[m + j]; | |
| } | |
| t[0] = t[m + mh]; | |
| m = mh; | |
| } | |
| a[l] = t[0]; | |
| } | |
| a[0] = 0; | |
| } | |
| /* -------- initializing routines -------- */ | |
| #include <math.h> | |
| void makewt(int nw, int *ip, float *w) | |
| { | |
| void bitrv2(int n, int *ip, float *a); | |
| int j, nwh; | |
| float delta, x, y; | |
| ip[0] = nw; | |
| ip[1] = 1; | |
| if (nw > 2) { | |
| nwh = nw >> 1; | |
| delta = atan(1.0) / nwh; | |
| w[0] = 1; | |
| w[1] = 0; | |
| w[nwh] = cos(delta * nwh); | |
| w[nwh + 1] = w[nwh]; | |
| if (nwh > 2) { | |
| for (j = 2; j < nwh; j += 2) { | |
| x = cos(delta * j); | |
| y = sin(delta * j); | |
| w[j] = x; | |
| w[j + 1] = y; | |
| w[nw - j] = y; | |
| w[nw - j + 1] = x; | |
| } | |
| bitrv2(nw, ip + 2, w); | |
| } | |
| } | |
| } | |
| void makect(int nc, int *ip, float *c) | |
| { | |
| int j, nch; | |
| float delta; | |
| ip[1] = nc; | |
| if (nc > 1) { | |
| nch = nc >> 1; | |
| delta = atan(1.0) / nch; | |
| c[0] = cos(delta * nch); | |
| c[nch] = 0.5 * c[0]; | |
| for (j = 1; j < nch; j++) { | |
| c[j] = 0.5 * cos(delta * j); | |
| c[nc - j] = 0.5 * sin(delta * j); | |
| } | |
| } | |
| } | |
| /* -------- child routines -------- */ | |
| void bitrv2(int n, int *ip, float *a) | |
| { | |
| int j, j1, k, k1, l, m, m2; | |
| float xr, xi, yr, yi; | |
| ip[0] = 0; | |
| l = n; | |
| m = 1; | |
| while ((m << 3) < l) { | |
| l >>= 1; | |
| for (j = 0; j < m; j++) { | |
| ip[m + j] = ip[j] + l; | |
| } | |
| m <<= 1; | |
| } | |
| m2 = 2 * m; | |
| if ((m << 3) == l) { | |
| for (k = 0; k < m; k++) { | |
| for (j = 0; j < k; j++) { | |
| j1 = 2 * j + ip[k]; | |
| k1 = 2 * k + ip[j]; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += 2 * m2; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 -= m2; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += 2 * m2; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| } | |
| j1 = 2 * k + m2 + ip[k]; | |
| k1 = j1 + m2; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| } | |
| } | |
| else { | |
| for (k = 1; k < m; k++) { | |
| for (j = 0; j < k; j++) { | |
| j1 = 2 * j + ip[k]; | |
| k1 = 2 * k + ip[j]; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += m2; | |
| xr = a[j1]; | |
| xi = a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| } | |
| } | |
| } | |
| } | |
| void bitrv2conj(int n, int *ip, float *a) | |
| { | |
| int j, j1, k, k1, l, m, m2; | |
| float xr, xi, yr, yi; | |
| ip[0] = 0; | |
| l = n; | |
| m = 1; | |
| while ((m << 3) < l) { | |
| l >>= 1; | |
| for (j = 0; j < m; j++) { | |
| ip[m + j] = ip[j] + l; | |
| } | |
| m <<= 1; | |
| } | |
| m2 = 2 * m; | |
| if ((m << 3) == l) { | |
| for (k = 0; k < m; k++) { | |
| for (j = 0; j < k; j++) { | |
| j1 = 2 * j + ip[k]; | |
| k1 = 2 * k + ip[j]; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += 2 * m2; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 -= m2; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += 2 * m2; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| } | |
| k1 = 2 * k + ip[k]; | |
| a[k1 + 1] = -a[k1 + 1]; | |
| j1 = k1 + m2; | |
| k1 = j1 + m2; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| k1 += m2; | |
| a[k1 + 1] = -a[k1 + 1]; | |
| } | |
| } | |
| else { | |
| a[1] = -a[1]; | |
| a[m2 + 1] = -a[m2 + 1]; | |
| for (k = 1; k < m; k++) { | |
| for (j = 0; j < k; j++) { | |
| j1 = 2 * j + ip[k]; | |
| k1 = 2 * k + ip[j]; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| j1 += m2; | |
| k1 += m2; | |
| xr = a[j1]; | |
| xi = -a[j1 + 1]; | |
| yr = a[k1]; | |
| yi = -a[k1 + 1]; | |
| a[j1] = yr; | |
| a[j1 + 1] = yi; | |
| a[k1] = xr; | |
| a[k1 + 1] = xi; | |
| } | |
| k1 = 2 * k + ip[k]; | |
| a[k1 + 1] = -a[k1 + 1]; | |
| a[k1 + m2 + 1] = -a[k1 + m2 + 1]; | |
| } | |
| } | |
| } | |
| void cftfsub(int n, float *a, float *w) | |
| { | |
| void cft1st(int n, float *a, float *w); | |
| void cftmdl(int n, int l, float *a, float *w); | |
| int j, j1, j2, j3, l; | |
| float x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i; | |
| l = 2; | |
| if (n > 8) { | |
| cft1st(n, a, w); | |
| l = 8; | |
| while ((l << 2) < n) { | |
| cftmdl(n, l, a, w); | |
| l <<= 2; | |
| } | |
| } | |
| if ((l << 2) == n) { | |
| for (j = 0; j < l; j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = a[j + 1] + a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = a[j + 1] - a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| a[j2] = x0r - x2r; | |
| a[j2 + 1] = x0i - x2i; | |
| a[j1] = x1r - x3i; | |
| a[j1 + 1] = x1i + x3r; | |
| a[j3] = x1r + x3i; | |
| a[j3 + 1] = x1i - x3r; | |
| } | |
| } | |
| else { | |
| for (j = 0; j < l; j += 2) { | |
| j1 = j + l; | |
| x0r = a[j] - a[j1]; | |
| x0i = a[j + 1] - a[j1 + 1]; | |
| a[j] += a[j1]; | |
| a[j + 1] += a[j1 + 1]; | |
| a[j1] = x0r; | |
| a[j1 + 1] = x0i; | |
| } | |
| } | |
| } | |
| void cftbsub(int n, float *a, float *w) | |
| { | |
| void cft1st(int n, float *a, float *w); | |
| void cftmdl(int n, int l, float *a, float *w); | |
| int j, j1, j2, j3, l; | |
| float x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i; | |
| l = 2; | |
| if (n > 8) { | |
| cft1st(n, a, w); | |
| l = 8; | |
| while ((l << 2) < n) { | |
| cftmdl(n, l, a, w); | |
| l <<= 2; | |
| } | |
| } | |
| if ((l << 2) == n) { | |
| for (j = 0; j < l; j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = -a[j + 1] - a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = -a[j + 1] + a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i - x2i; | |
| a[j2] = x0r - x2r; | |
| a[j2 + 1] = x0i + x2i; | |
| a[j1] = x1r - x3i; | |
| a[j1 + 1] = x1i - x3r; | |
| a[j3] = x1r + x3i; | |
| a[j3 + 1] = x1i + x3r; | |
| } | |
| } | |
| else { | |
| for (j = 0; j < l; j += 2) { | |
| j1 = j + l; | |
| x0r = a[j] - a[j1]; | |
| x0i = -a[j + 1] + a[j1 + 1]; | |
| a[j] += a[j1]; | |
| a[j + 1] = -a[j + 1] - a[j1 + 1]; | |
| a[j1] = x0r; | |
| a[j1 + 1] = x0i; | |
| } | |
| } | |
| } | |
| void cft1st(int n, float *a, float *w) | |
| { | |
| int j, k1, k2; | |
| float wk1r, wk1i, wk2r, wk2i, wk3r, wk3i; | |
| float x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i; | |
| x0r = a[0] + a[2]; | |
| x0i = a[1] + a[3]; | |
| x1r = a[0] - a[2]; | |
| x1i = a[1] - a[3]; | |
| x2r = a[4] + a[6]; | |
| x2i = a[5] + a[7]; | |
| x3r = a[4] - a[6]; | |
| x3i = a[5] - a[7]; | |
| a[0] = x0r + x2r; | |
| a[1] = x0i + x2i; | |
| a[4] = x0r - x2r; | |
| a[5] = x0i - x2i; | |
| a[2] = x1r - x3i; | |
| a[3] = x1i + x3r; | |
| a[6] = x1r + x3i; | |
| a[7] = x1i - x3r; | |
| wk1r = w[2]; | |
| x0r = a[8] + a[10]; | |
| x0i = a[9] + a[11]; | |
| x1r = a[8] - a[10]; | |
| x1i = a[9] - a[11]; | |
| x2r = a[12] + a[14]; | |
| x2i = a[13] + a[15]; | |
| x3r = a[12] - a[14]; | |
| x3i = a[13] - a[15]; | |
| a[8] = x0r + x2r; | |
| a[9] = x0i + x2i; | |
| a[12] = x2i - x0i; | |
| a[13] = x0r - x2r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[10] = wk1r * (x0r - x0i); | |
| a[11] = wk1r * (x0r + x0i); | |
| x0r = x3i + x1r; | |
| x0i = x3r - x1i; | |
| a[14] = wk1r * (x0i - x0r); | |
| a[15] = wk1r * (x0i + x0r); | |
| k1 = 0; | |
| for (j = 16; j < n; j += 16) { | |
| k1 += 2; | |
| k2 = 2 * k1; | |
| wk2r = w[k1]; | |
| wk2i = w[k1 + 1]; | |
| wk1r = w[k2]; | |
| wk1i = w[k2 + 1]; | |
| wk3r = wk1r - 2 * wk2i * wk1i; | |
| wk3i = 2 * wk2i * wk1r - wk1i; | |
| x0r = a[j] + a[j + 2]; | |
| x0i = a[j + 1] + a[j + 3]; | |
| x1r = a[j] - a[j + 2]; | |
| x1i = a[j + 1] - a[j + 3]; | |
| x2r = a[j + 4] + a[j + 6]; | |
| x2i = a[j + 5] + a[j + 7]; | |
| x3r = a[j + 4] - a[j + 6]; | |
| x3i = a[j + 5] - a[j + 7]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| x0r -= x2r; | |
| x0i -= x2i; | |
| a[j + 4] = wk2r * x0r - wk2i * x0i; | |
| a[j + 5] = wk2r * x0i + wk2i * x0r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[j + 2] = wk1r * x0r - wk1i * x0i; | |
| a[j + 3] = wk1r * x0i + wk1i * x0r; | |
| x0r = x1r + x3i; | |
| x0i = x1i - x3r; | |
| a[j + 6] = wk3r * x0r - wk3i * x0i; | |
| a[j + 7] = wk3r * x0i + wk3i * x0r; | |
| wk1r = w[k2 + 2]; | |
| wk1i = w[k2 + 3]; | |
| wk3r = wk1r - 2 * wk2r * wk1i; | |
| wk3i = 2 * wk2r * wk1r - wk1i; | |
| x0r = a[j + 8] + a[j + 10]; | |
| x0i = a[j + 9] + a[j + 11]; | |
| x1r = a[j + 8] - a[j + 10]; | |
| x1i = a[j + 9] - a[j + 11]; | |
| x2r = a[j + 12] + a[j + 14]; | |
| x2i = a[j + 13] + a[j + 15]; | |
| x3r = a[j + 12] - a[j + 14]; | |
| x3i = a[j + 13] - a[j + 15]; | |
| a[j + 8] = x0r + x2r; | |
| a[j + 9] = x0i + x2i; | |
| x0r -= x2r; | |
| x0i -= x2i; | |
| a[j + 12] = -wk2i * x0r - wk2r * x0i; | |
| a[j + 13] = -wk2i * x0i + wk2r * x0r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[j + 10] = wk1r * x0r - wk1i * x0i; | |
| a[j + 11] = wk1r * x0i + wk1i * x0r; | |
| x0r = x1r + x3i; | |
| x0i = x1i - x3r; | |
| a[j + 14] = wk3r * x0r - wk3i * x0i; | |
| a[j + 15] = wk3r * x0i + wk3i * x0r; | |
| } | |
| } | |
| void cftmdl(int n, int l, float *a, float *w) | |
| { | |
| int j, j1, j2, j3, k, k1, k2, m, m2; | |
| float wk1r, wk1i, wk2r, wk2i, wk3r, wk3i; | |
| float x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i; | |
| m = l << 2; | |
| for (j = 0; j < l; j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = a[j + 1] + a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = a[j + 1] - a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| a[j2] = x0r - x2r; | |
| a[j2 + 1] = x0i - x2i; | |
| a[j1] = x1r - x3i; | |
| a[j1 + 1] = x1i + x3r; | |
| a[j3] = x1r + x3i; | |
| a[j3 + 1] = x1i - x3r; | |
| } | |
| wk1r = w[2]; | |
| for (j = m; j < l + m; j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = a[j + 1] + a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = a[j + 1] - a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| a[j2] = x2i - x0i; | |
| a[j2 + 1] = x0r - x2r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[j1] = wk1r * (x0r - x0i); | |
| a[j1 + 1] = wk1r * (x0r + x0i); | |
| x0r = x3i + x1r; | |
| x0i = x3r - x1i; | |
| a[j3] = wk1r * (x0i - x0r); | |
| a[j3 + 1] = wk1r * (x0i + x0r); | |
| } | |
| k1 = 0; | |
| m2 = 2 * m; | |
| for (k = m2; k < n; k += m2) { | |
| k1 += 2; | |
| k2 = 2 * k1; | |
| wk2r = w[k1]; | |
| wk2i = w[k1 + 1]; | |
| wk1r = w[k2]; | |
| wk1i = w[k2 + 1]; | |
| wk3r = wk1r - 2 * wk2i * wk1i; | |
| wk3i = 2 * wk2i * wk1r - wk1i; | |
| for (j = k; j < l + k; j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = a[j + 1] + a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = a[j + 1] - a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| x0r -= x2r; | |
| x0i -= x2i; | |
| a[j2] = wk2r * x0r - wk2i * x0i; | |
| a[j2 + 1] = wk2r * x0i + wk2i * x0r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[j1] = wk1r * x0r - wk1i * x0i; | |
| a[j1 + 1] = wk1r * x0i + wk1i * x0r; | |
| x0r = x1r + x3i; | |
| x0i = x1i - x3r; | |
| a[j3] = wk3r * x0r - wk3i * x0i; | |
| a[j3 + 1] = wk3r * x0i + wk3i * x0r; | |
| } | |
| wk1r = w[k2 + 2]; | |
| wk1i = w[k2 + 3]; | |
| wk3r = wk1r - 2 * wk2r * wk1i; | |
| wk3i = 2 * wk2r * wk1r - wk1i; | |
| for (j = k + m; j < l + (k + m); j += 2) { | |
| j1 = j + l; | |
| j2 = j1 + l; | |
| j3 = j2 + l; | |
| x0r = a[j] + a[j1]; | |
| x0i = a[j + 1] + a[j1 + 1]; | |
| x1r = a[j] - a[j1]; | |
| x1i = a[j + 1] - a[j1 + 1]; | |
| x2r = a[j2] + a[j3]; | |
| x2i = a[j2 + 1] + a[j3 + 1]; | |
| x3r = a[j2] - a[j3]; | |
| x3i = a[j2 + 1] - a[j3 + 1]; | |
| a[j] = x0r + x2r; | |
| a[j + 1] = x0i + x2i; | |
| x0r -= x2r; | |
| x0i -= x2i; | |
| a[j2] = -wk2i * x0r - wk2r * x0i; | |
| a[j2 + 1] = -wk2i * x0i + wk2r * x0r; | |
| x0r = x1r - x3i; | |
| x0i = x1i + x3r; | |
| a[j1] = wk1r * x0r - wk1i * x0i; | |
| a[j1 + 1] = wk1r * x0i + wk1i * x0r; | |
| x0r = x1r + x3i; | |
| x0i = x1i - x3r; | |
| a[j3] = wk3r * x0r - wk3i * x0i; | |
| a[j3 + 1] = wk3r * x0i + wk3i * x0r; | |
| } | |
| } | |
| } | |
| void rftfsub(int n, float *a, int nc, float *c) | |
| { | |
| int j, k, kk, ks, m; | |
| float wkr, wki, xr, xi, yr, yi; | |
| m = n >> 1; | |
| ks = 2 * nc / m; | |
| kk = 0; | |
| for (j = 2; j < m; j += 2) { | |
| k = n - j; | |
| kk += ks; | |
| wkr = 0.5 - c[nc - kk]; | |
| wki = c[kk]; | |
| xr = a[j] - a[k]; | |
| xi = a[j + 1] + a[k + 1]; | |
| yr = wkr * xr - wki * xi; | |
| yi = wkr * xi + wki * xr; | |
| a[j] -= yr; | |
| a[j + 1] -= yi; | |
| a[k] += yr; | |
| a[k + 1] -= yi; | |
| } | |
| } | |
| void rftbsub(int n, float *a, int nc, float *c) | |
| { | |
| int j, k, kk, ks, m; | |
| float wkr, wki, xr, xi, yr, yi; | |
| a[1] = -a[1]; | |
| m = n >> 1; | |
| ks = 2 * nc / m; | |
| kk = 0; | |
| for (j = 2; j < m; j += 2) { | |
| k = n - j; | |
| kk += ks; | |
| wkr = 0.5 - c[nc - kk]; | |
| wki = c[kk]; | |
| xr = a[j] - a[k]; | |
| xi = a[j + 1] + a[k + 1]; | |
| yr = wkr * xr + wki * xi; | |
| yi = wkr * xi - wki * xr; | |
| a[j] -= yr; | |
| a[j + 1] = yi - a[j + 1]; | |
| a[k] += yr; | |
| a[k + 1] = yi - a[k + 1]; | |
| } | |
| a[m + 1] = -a[m + 1]; | |
| } | |
| void dctsub(int n, float *a, int nc, float *c) | |
| { | |
| int j, k, kk, ks, m; | |
| float wkr, wki, xr; | |
| m = n >> 1; | |
| ks = nc / n; | |
| kk = 0; | |
| for (j = 1; j < m; j++) { | |
| k = n - j; | |
| kk += ks; | |
| wkr = c[kk] - c[nc - kk]; | |
| wki = c[kk] + c[nc - kk]; | |
| xr = wki * a[j] - wkr * a[k]; | |
| a[j] = wkr * a[j] + wki * a[k]; | |
| a[k] = xr; | |
| } | |
| a[m] *= c[0]; | |
| } | |
| void dstsub(int n, float *a, int nc, float *c) | |
| { | |
| int j, k, kk, ks, m; | |
| float wkr, wki, xr; | |
| m = n >> 1; | |
| ks = nc / n; | |
| kk = 0; | |
| for (j = 1; j < m; j++) { | |
| k = n - j; | |
| kk += ks; | |
| wkr = c[kk] - c[nc - kk]; | |
| wki = c[kk] + c[nc - kk]; | |
| xr = wki * a[k] - wkr * a[j]; | |
| a[k] = wkr * a[k] + wki * a[j]; | |
| a[j] = xr; | |
| } | |
| a[m] *= c[0]; | |
| } | |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| // pin P1.6:15 - Serial data out (MOSI) | |
| // pin P1.5: 7 - Serial clock out (SCLK) | |
| // pin P5.7:17 - Data/Command select (RS or A0) | |
| // pin P2.5:19 - LCD reset (RST) | |
| // pin P3.0:18 - LCD chip select (CS) | |
| ST7565 lcd(15, 7, 17, 19, 18); | |
| void setupTaskLCD() { | |
| lcd.begin(0x1c); | |
| lcd.clear(); | |
| } | |
| void loopTaskLCD() { | |
| char sbuf[10]; | |
| lcd.clear(); | |
| for (int i = 1; i < NMAX/2; i++) { | |
| lcd.drawline(i, LCDHEIGHT-1, i, LCDBuffer[i], BLACK); | |
| } | |
| sprintf(sbuf, "%4d", FFTCounter); | |
| lcd.drawstring(10, 0, sbuf); | |
| lcd.display(); | |
| delay(100); | |
| } |
0 件のコメント:
コメントを投稿
注: コメントを投稿できるのは、このブログのメンバーだけです。