OSDN Git Service

[v1.0][Doc] v1.0(.0) Release.
[openi2cradio/OpenI2CRadio.git] / akc6955.c
index bacda65..6bba528 100644 (file)
--- a/akc6955.c
+++ b/akc6955.c
 
 #include <stdarg.h>
 #include <stdio.h>
+#ifdef __SDCC
 #include <delay.h>
+#else
+#include <xc.h>
+#endif
 #include <string.h>
 #include "akc6955.h"
 #include "i2c_io.h"
 #include "idle.h"
+#include "power.h"
 #include "commondef.h"
+#include "menu.h"
+
 
-void akc6955_writecmd(unsigned char reg, unsigned char data)
+unsigned char akc6955_get_amband(void)
 {
-//    OPENASMASTER();
-    i2c_open(I2C_MASTER, I2C_SLEW_OFF, 19);
-//    i2c_idle();
-//    delay1ktcy(8);
-    I2C_START();
-    i2c_writechar(0x20);
-    //delay1ktcy(8);
-    i2c_writechar(reg);
-    //delay1ktcy(8);
-    i2c_writechar(data);
-    //delay1ktcy(8);
-    I2C_STOP();
- //   delay1ktcy(8);
-    i2c_close();
-//    CLOSEASMASTER();
+   return akc6955_get_band() >> 3;
 }
 
-unsigned char akc6955_readcmd(unsigned char reg)
+unsigned char akc6955_get_fmband(void)
 {
-    unsigned char c;
-    //    OPENASMASTER();
-    i2c_open(I2C_MASTER, I2C_SLEW_OFF, 19);
-//    i2c_idle();
-//    delay1ktcy(8);
-    I2C_START();
-    i2c_writechar(0x20);
-  //  delay1ktcy(8);
-    i2c_writechar(reg);
-  //  delay1ktcy(8);
-    I2C_STOP();
-    delay100tcy(1);
-    I2C_START();
-    i2c_writechar(0x21);
-  //  delay1ktcy(8);
-    c = i2c_readchar();
-    I2C_ACK();
-    I2C_STOP();
- //   delay1ktcy(8);
-    i2c_close();
-//    CLOSEASMASTER();
-
-    return c;
+   return akc6955_get_band() & 0x07;
 }
 
-void akc6955_chg_fm(unsigned char f)
+void akc6955_chg_fm(unsigned char f, unsigned int freq)
 {
     __bitops_t b;
     b.byte = akc6955_readcmd(AKC6955_POWER);
+    b.b6 = 0;
     if(f != 0){
-//        b |= 0x40;
         b.b6 = 1;
-    } else {
-        b.b6 = 0;
+        akc6955_set_fmband(setup.fmband);
+        akc6955_writecmd(AKC6955_POWER, b.byte);
+        akc6955_set_freq(freq);
+        return;
     }
     akc6955_writecmd(AKC6955_POWER, b.byte);
+    akc6955_set_amband(setup.amband);
+    akc6955_set_freq(freq);
+
 }
 
 unsigned char akc6955_get_fm(void)
@@ -108,61 +84,63 @@ unsigned char akc6955_get_fm(void)
 void akc6955_set_amband(unsigned char band)
 {
     unsigned char b;
+    unsigned char c;
+
+    c = RFAMP_SW;
+    if((band < AKC6955_BAND_SW1) || (band == AKC6955_BAND_MW4)) {
+        c = RFAMP_MWLW;
+    }
+    rfamp_power(c);
     b = akc6955_readcmd(AKC6955_BAND);
     b &= 0x07; // extract FM
     b = b | ((band & 0x1f) << 3);
     akc6955_writecmd(AKC6955_BAND, b);
+    akc6955_do_tune();
 }
 
 void akc6955_set_fmband(unsigned char band)
 {
     unsigned char b;
+    rfamp_power(RFAMP_FM);
     b = akc6955_readcmd(AKC6955_BAND);
     b &= 0xf8; // extract AM
     b = b | (band & 0x07);
     akc6955_writecmd(AKC6955_BAND, b);
+    akc6955_do_tune();
 }
 
-unsigned char akc6955_get_amband(void)
+unsigned char akc6955_get_band(void)
 {
-    unsigned char b;
-    b = akc6955_readcmd(AKC6955_BAND);
-    b = (b & 0xf8) >> 3;
-    return b;
+    return akc6955_readcmd(AKC6955_BAND);
 }
 
-unsigned char akc6955_get_fmband(void)
-{
-    unsigned char b;
-    b = akc6955_readcmd(AKC6955_BAND);
-    b = b & 0x07;
-    return b;
-}
 
-#if 1
 void akc6955_set_power(unsigned char on)
 {
     __bitops_t b;
     b.byte = akc6955_readcmd(AKC6955_POWER);
+    b.b7 = 0;
     if(on != 0){
         b.b7 = 1;
-        b.b2 = 1;
-    } else {
-        b.b7 = 0;
     }
     akc6955_writecmd(AKC6955_POWER, b.byte);
 }
-#endif
 
 void akc6955_do_tune(void)
 {
     __bitops_t b;
     b.byte = akc6955_readcmd(AKC6955_POWER);
     b.b5 = 0; // Tun = '0'
+    b.b4 = 0; // Force abort scan.
     akc6955_writecmd(AKC6955_POWER, b.byte);
-    // Need sleep?
+    idle_time_ms(1);
+    // Need sleep?-> Need! this sequence re                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 fer from reference code.
     b.b5= 1; // Tun = '1'
     akc6955_writecmd(AKC6955_POWER, b.byte);
+    idle_time_ms(1);
+    b.b5 = 0;
+    akc6955_writecmd(AKC6955_POWER, b.byte);
+    idle_time_ms(8);
 }
 
 unsigned char akc6955_tune(void)
@@ -175,20 +153,30 @@ unsigned char akc6955_tune(void)
     return 0;
 }
 
+unsigned char akc6955_get_mode3k(void)
+{
+    __bitops_t c;
+    c.byte = akc6955_readcmd(AKC6955_CNR_AM);
+    if(c.b7) {
+        return 0xff;
+    }
+    return 0;
+}
+
 
-unsigned int akc6955_mode3k(unsigned char flag)
+void akc6955_mode3k(unsigned char flag)
 {
     __bitops_t b;
+    b.byte = akc6955_readcmd(AKC6955_CH_LO);
+    akc6955_writecmd(AKC6955_CH_LO, b.byte);
+
     b.byte = akc6955_readcmd(AKC6955_CH_HI);
+    b.b5 = 0;
     if(flag != 0){
         b.b5 = 1;
-    } else {
-        b.b5 = 1;
     }
     akc6955_writecmd(AKC6955_CH_HI, b.byte);
     akc6955_do_tune();
-    _AKC6955_WAIT_62_5MS();
-    return akc6955_get_freq();
 }
 
 void akc6955_set_tune(unsigned char mode_3k, unsigned int ch)
@@ -197,48 +185,100 @@ void akc6955_set_tune(unsigned char mode_3k, unsigned int ch)
     __bitops_t f;
     __bitops_t b;
     unsigned int i = ch;
-
+    unsigned char comp;
+    unsigned int start;
+    unsigned int stop;
+    unsigned int q;
+
+    do { // Wait for before completed
+        comp = akc6955_chk_donescan();
+        idle_time_35ms();
+    } while(comp == 0x00);
+   
     f.byte = akc6955_readcmd(AKC6955_POWER);
-    if(f.b6){
-        band = 0;
-        // AM
+    if(!f.b6){ // AM
+       band = akc6955_get_amband(); 
+       if(band == AKC6955_BAND_MW2){
+         // BAND=AM && MW2
+         i = ch / 3; // On MW2, Channnel must be multiple of 3.
+         i = i * 3; // i = (i/3) * 3
+         mode_3k = 0xff;
+         q = 3;
+       } else if(mode_3k != 0) {
+         q = 3;
+       } else {
+         q = 5;
+       }
+       
+        if(band == AKC6955_BAND_AMUSER) {
+           start = setup.am_usrbands[setup.am_userbandnum].start * 32;
+           stop = setup.am_usrbands[setup.am_userbandnum].stop * 32;
+       } else {
+           start = ambands[band].start / q;
+           stop = ambands[band].end / q;
+       }
     } else {
-        band = akc6955_get_amband();
-    }
-
-    if(band == 2){
-        // BAND=AM && MW2
-        i = ch / 3; // On MW2, Channnel must be multiple of 3.
-        i = i * 3; // i = (i/3) * 3
+       band = akc6955_get_fmband(); 
+        if(band == AKC6955_BAND_FMUSER) {
+           start = setup.fm_usrbands[setup.fm_userbandnum].start * 32;
+           stop = setup.fm_usrbands[setup.fm_userbandnum].stop * 32;
+       } else {
+           start = ((fmbands[band].start - 3000) * 4) / 10;
+           stop =  ((fmbands[band].end - 3000) * 4)  / 10;
+       }
     }
+   
     if(i > 0x1fff) i = 0x1fff;
-    //i = ch & 0x1fff;
+    if(i > stop) i = stop;
+    if(i < start) i = start;
+
+    b.byte = i & 0xff;
+    akc6955_writecmd(AKC6955_CH_LO, b.byte);
 
     b.byte = i >> 8;
     b.b6 = 1;
-    if(mode_3k != 0){
+    if((setup.fm == 0) && (mode_3k != 0)){
         b.b5 = 1; // Mode 3K ON
+    } else {
+       b.b5 = 0;
     }
+
     akc6955_writecmd(AKC6955_CH_HI, b.byte);
 
-    b.byte = i & 0xff;
-    akc6955_writecmd(AKC6955_CH_LO, b.byte);
     akc6955_do_tune();
 }
 
 void akc6955_do_scan(unsigned char up)
 {
     __bitops_t b;
-    akc6955_do_tune();
+
+    //20130823 Need wait for scan/tune completed w/o SDCC 3.3.x.
+#ifndef __SDCC
+    while(akc6955_chk_donescan() == 0)
+    {
+        idle_time_ms(5);
+    }
+#endif
     b.byte = akc6955_readcmd(AKC6955_POWER);
     b.b3 = 0;
     b.b4 = 0;
+// 20130823 : Is this collect?
+#ifdef __SDCC
+     b.b5 = 0;
+    akc6955_writecmd(AKC6955_POWER, b.byte);
+    idle_time_ms(35);
+#endif
+
+    b.b5 = 1; // Tune 0->1.
+    akc6955_writecmd(AKC6955_POWER, b.byte);
+    idle_time_ms(35);
+
+    b.b4 = 1;
     if(up != 0) {
         b.b3= 1;
     }
-    akc6955_writecmd(AKC6955_POWER, b.byte); // Falldown seek bit to '0'.
-    b.b4 = 1;
     akc6955_writecmd(AKC6955_POWER, b.byte); // Raise seek bit to '1'.
+    idle_time_ms(35);
 }
 
 void akc6955_abort_scan(void)
@@ -257,40 +297,55 @@ void akc6955_set_scanrate_fm(unsigned char rate)
     akc6955_writecmd(AKC6955_SPACE, c);
 }
 
+unsigned char akc6955_get_scanrate_fm(void)
+{
+    unsigned char c;
+    c = akc6955_readcmd(AKC6955_SPACE);
+    c = (c & 0x30) >> 4;
+   return c;
+}
+
 unsigned char akc6955_chk_donescan(void)
 {
-    unsigned char b;
-    b = akc6955_readcmd(AKC6955_RCH_HI) & 0x40;
-    return b;
+    __bitops_t b;
+    b.byte = akc6955_readcmd(AKC6955_RCH_HI);
+    if(b.b6){
+        return 0xff;
+    }
+    return 0;
 }
 
+unsigned int akc6955_get_channel(void)
+{
+    unsigned char h, l;
+    unsigned int u;
+    l = akc6955_readcmd(AKC6955_RCH_LO) ;
+    h = akc6955_readcmd(AKC6955_RCH_HI) & 0x1f;
+    u = (h << 8) | l;
+    return u;
+}
 
 /*
  * Get AM/FM Frequency: ret = KHz at AM, 10KHz @FM.
  */
 unsigned int akc6955_get_freq(void)
 {
-    unsigned char h, l;
     __bitops_t f, b;
     unsigned int i;
     unsigned int freq;
+    unsigned char band;
 
-//    b = akc6955_readcmd(AKC6955_CNR_AM) & 0x80;
-    l = akc6955_readcmd(AKC6955_RCH_LO) ;
-    h = akc6955_readcmd(AKC6955_RCH_HI) & 0x1f;
     f.byte = akc6955_readcmd(AKC6955_POWER);
-    i = l + (h << 8);
+    i = akc6955_get_channel();
     if(f.b6){ // 25KHz
         freq = (i * 5) / 2 + 3000; // freq' = 25*i[KHz] = (25 / 10) *i [10KHz]
-                                       // = 2.5i[10KHz]
-                                       // freq = freq' + 30MHz = freq' + 3000[10KHz]
     } else { // 5K
        b.byte = akc6955_readcmd(AKC6955_CH_HI);
-        if(b.b5) { // 5KHz
-           freq = i * 3; // freq = 5*i[KHz] = (4+1)*i[KHz]
-        } else { // 3KHz
-           freq = i * 5; // freq = 3i[KHz] = (2+1)i[KHz]
-        }
+       band = akc6955_get_amband();
+       freq = i * 5;
+       if((band == AKC6955_BAND_MW2) || (b.b5)){
+           freq = i * 3;
+       }
     }
     return freq;
 }
@@ -305,23 +360,22 @@ void akc6955_set_freq(unsigned int freq)
     f.byte = akc6955_readcmd(AKC6955_POWER);
     if(f.b6) { // FM
         band = akc6955_get_fmband();
-        if(freq <  fmbands[band].start) freq = fmbands[band].start;
-        if(freq >= fmbands[band].end)   freq = fmbands[band].end - 1;
         ch = freq - 3000;
-        ch = (ch * 4) / 10;
+        ch = (ch << 2) / 10;
     } else {
         band = akc6955_get_amband();
-        if(band >= AKC6955_BAND_AMEND) band = AKC6955_BAND_AMEND - 1;
-        if(freq <  ambands[band].start) freq = ambands[band].start;
-        if(freq >= ambands[band].end)   freq = ambands[band].end - 1;
         mode3k.byte = akc6955_readcmd(AKC6955_CNR_AM);
-        if(mode3k.b7){
+        if(band == AKC6955_BAND_MW2) {
+            ch = (freq / 9) * 3; // See datasheet.
+        } else if(band == AKC6955_BAND_MW3) {
+            ch = freq / 5;
+        } else  if(mode3k.b7){
             ch = freq / 3;
         } else {
             ch = freq / 5;
         }
     }
-    akc6955_set_tune(mode3k.byte, ch);
+    akc6955_set_tune(mode3k.b7, ch);
 }
 
 void akc6955_set_userband(unsigned char start, unsigned char stop, unsigned int ch, unsigned char mode3k)
@@ -386,80 +440,65 @@ int akc6955_read_level(void)
     return level;
 }
 
+
 // Get diff. unit = 100Hz.
 int akc6955_get_diff(void)
 {
-    unsigned char diff;
+    __bitops_t diff;
     __bitops_t f;
     int n;
-    diff = akc6955_readcmd(AKC6955_FDNUM);
-    if(diff  <= 127) {
-        n = (int)diff & 0x007f;
+
+    diff.byte = akc6955_readcmd(AKC6955_FDNUM);
+    if(diff.b7) {
+        diff.b7 = 0;
+        n = -((int) diff.byte);
     } else {
-        n = -((int)(diff & 0x007f));
+        diff.b7 = 0;
+        n = (int)diff.byte;
     }
+
     f.byte = akc6955_readcmd(AKC6955_POWER);
-    if(f.b6) { // AM
+    if(f.b6) { // FM
         return n * 10;
-    } else {
-        return n; // 10n
     }
+    return n; // 10n
 }
 
-unsigned int akc6955_up_freq(unsigned int step)
+void akc6955_up_freq(unsigned int step)
 {
     unsigned int freq;
-    __bitops_t f;
-    unsigned char band;
+    __bitops_t mode3k;
 
-    f.byte = akc6955_readcmd(AKC6955_POWER);
-    freq = akc6955_get_freq();
+    freq = akc6955_get_channel();
     freq += step;
-    if(f.b6){
-        band = akc6955_get_fmband();
-        if(band >= AKC6955_BAND_FMEND) band = AKC6955_BAND_FMEND - 1;
-        if(freq >= fmbands[band].end) freq = fmbands[band].end - 1;
-    } else {
-        band = akc6955_get_amband();
-        if(band >= AKC6955_BAND_AMEND) band = AKC6955_BAND_AMEND - 1;
-        if(freq >= ambands[band].end) freq = ambands[band].end - 1;
-    }
-    akc6955_set_freq(freq);
-    return akc6955_get_freq();
+//    mode3k.byte = akc6955_readcmd(AKC6955_CNR_AM);
+//    akc6955_set_tune(mode3k.b7, freq);
+    akc6955_set_tune(setup.am_mode3k, freq);
 }
 
 
-unsigned int akc6955_down_freq(unsigned int step)
+void akc6955_down_freq(unsigned int step)
 {
     unsigned int freq;
-    __bitops_t f;
-    unsigned char band;
+    __bitops_t mode3k;
 
-    freq = akc6955_get_freq();
-    if(freq <= step) return freq;
+    freq = akc6955_get_channel();
+    if(freq <= step) return;
     freq -= step;
-
-    f.byte = akc6955_readcmd(AKC6955_POWER);
-    if(f.b6 == 0){
-        band = akc6955_get_amband();
-        if(band >= AKC6955_BAND_AMEND) band = AKC6955_BAND_AMEND - 1;
-        if(freq < ambands[band].start) freq = ambands[band].start;
-    } else {
-        band = akc6955_get_fmband();
-        if(band >= AKC6955_BAND_FMEND) band = AKC6955_BAND_FMEND - 1;
-        if(freq < fmbands[band].start) freq = fmbands[band].start;
-    }
-    akc6955_set_freq(freq);
-    return akc6955_get_freq();
+//    mode3k.byte = akc6955_readcmd(AKC6955_CNR_AM);
+//    akc6955_set_tune(mode3k.b7, freq);
+    akc6955_set_tune(setup.am_mode3k, freq);
 }
+
 void akc6955_setvolumemode(unsigned char flag)
 {
-    unsigned char c;
-    c = akc6955_readcmd(AKC6955_ENABLE) & 0xf7;
+    __bitops_t c;
+    c.byte = akc6955_readcmd(AKC6955_ENABLE);
+    c.b3 = 0;
     if(flag != 0x00){
-        c |= 0x08;
+        c.b3 = 1;
     }
-    akc6955_writecmd(AKC6955_ENABLE, c);
+    akc6955_writecmd(AKC6955_ENABLE, c.byte);
 }
 
 unsigned char akc6955_getvolumemode(void)
@@ -474,16 +513,16 @@ unsigned char akc6955_getvolumemode(void)
 
 void akc6955_setvolume(unsigned char level)
 {
-    unsigned char c;
-    c = akc6955_readcmd(AKC6955_VOLUME) & 0x03;
+//    unsigned char c;
+//    c = akc6955_readcmd(AKC6955_VOLUME) & 0x03;
     if(level > 63) level = 63;
-    akc6955_writecmd(AKC6955_VOLUME, (c | (level << 2)));
+    akc6955_writecmd(AKC6955_VOLUME, ((akc6955_readcmd(AKC6955_VOLUME) & 0x03) | (level << 2)));
 }
 
 unsigned char akc6955_getvolume(void)
 {
     unsigned char c;
-    c = (akc6955_readcmd(AKC6955_VOLUME) & 0xfc) >> 2;
+    c = akc6955_readcmd(AKC6955_VOLUME) >> 2;
     return c;
 }
 
@@ -506,40 +545,36 @@ unsigned char akc6955_get_prevolume(void)
 
 void akc6955_setphase(unsigned char flag)
 {
-    unsigned char c;
-    c = akc6955_readcmd(AKC6955_VOLUME);
+    __bitops_t c;
+    c.byte = akc6955_readcmd(AKC6955_VOLUME);
 
+    c.b0 = 1;
     if(flag == 0) {
-        c = c & 0xfe; //
-    } else {
-        c = c | 0x01;
+        c.b0 = 0; //
     }
-    akc6955_writecmd(AKC6955_VOLUME, c);
+    akc6955_writecmd(AKC6955_VOLUME, c.byte);
 }
 
 void akc6955_setline(unsigned char flag)
 {
-    unsigned char c;
-    c = akc6955_readcmd(AKC6955_VOLUME);
-
+    __bitops_t c;
+    c.byte = akc6955_readcmd(AKC6955_VOLUME);
+    c.b1 = 1;
     if(flag == 0) {
-        c = c & 0xfd; //
-    } else {
-        c = c | 0x02;
+        c.b1 = 0;
     }
-    akc6955_writecmd(AKC6955_VOLUME, c);
+    akc6955_writecmd(AKC6955_VOLUME, c.byte);
 }
 
 void akc6955_set_lowboost(unsigned char flag)
 {
-    unsigned char c;
-    c = akc6955_readcmd(AKC6955_STEREO);
+    __bitops_t c;
+    c.byte = akc6955_readcmd(AKC6955_STEREO);
+    c.b3 = 1;
     if(flag == 0) {
-        c &= 0xf7;
-    } else {
-        c |= 0x08;
+        c.b3 = 0;
     }
-    akc6955_writecmd(AKC6955_STEREO, c);
+    akc6955_writecmd(AKC6955_STEREO, c.byte);
 }
 
 void akc6955_set_stereomode(unsigned char mode)
@@ -572,22 +607,41 @@ unsigned int akc6955_get_battery(void)
     return batt;
 }
 
-/*
- * 0 = 150KHz
- * 1 = 200KHz
- * 2 = 50KHz
- * 3 = 100KHz
- */
-#if 0
-void akc6955_set_fmbandwidth(unsigned char bw)
+void akc6955_set_thresh_fmstereo(unsigned char a)
 {
-    unsigned char c = akc6955_readcmd(AKC6955_STEREO);
-    c = (c & 0xfc) | (bw & 0x03);
-    akc6955_writecmd(AKC6955_STEREO, c);
+    unsigned char b;
+    a = a & 0x03;
+    setup.threshold_fmstereo = a;
+    b = akc6955_readcmd(AKC6955_THRESH) & 0xfc;
+    akc6955_writecmd(AKC6955_THRESH, a | b);
 }
-unsigned char akc6955_get_fmbandwidth(void)
+
+void akc6955_set_thresh_width(unsigned char a)
 {
-    unsigned char c = akc6955_readcmd(AKC6955_STEREO) & 0x03;
-    return c;
+    unsigned char b;
+    a = a & 0x03;
+    setup.threshold_width = a;
+    a = a << 2; // << 2
+    b = akc6955_readcmd(AKC6955_THRESH) & 0xf3;;
+    akc6955_writecmd(AKC6955_THRESH, a | b);
+}
+
+void akc6955_set_thresh_amcnr(unsigned char a)
+{
+    unsigned char b;
+    a = a & 0x03;
+    setup.threshold_amcnr = a;
+    a = a << 4; // << 4
+    b = akc6955_readcmd(AKC6955_THRESH) & 0xcf;
+    akc6955_writecmd(AKC6955_THRESH, a | b);
+}
+
+void akc6955_set_thresh_fmcnr(unsigned char a)
+{
+    unsigned char b;
+    a = a & 0x03;
+    setup.threshold_fmcnr = a;
+    a = a << 6; // << 4
+    b = akc6955_readcmd(AKC6955_THRESH) & 0x3f;
+    akc6955_writecmd(AKC6955_THRESH, a | b);
 }
-#endif
\ No newline at end of file