Leon Chu
Published © GPL3+

Animated Musical Juke Box Player

Play list of favorite songs to animated lights and bobbing plush decor.

IntermediateWork in progress4 hours338

Things used in this project

Story

Read more

Schematics

Wire connections

Code

JukeBox

C/C++
 /* 
 *  MP3 JukeBox plays randomized songs   @chuartdo
 */

/* Peripheral configuration */
//#define GPS_TIME  
//#define SERVO   
//#define RGB_LCD
//#define ROTARY_ENCODER

/***************************************************************************** 
 *  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 St, Fifth Floor, Boston,  MA 02110-1301  USA
 */

#include <SDHCI.h>
#include <MediaPlayer.h>
#include <OutputMixer.h>
#include <MemoryUtil.h>
#include <MediaRecorder.h>

MediaRecorder *theRecorder;

static const int32_t recoding_frames = 400;
static const int32_t buffer_size = 512;  /*Now MP3 is 96kbps,48kHz. so, One frame is 288 bytes */
static uint8_t       s_buffer[buffer_size];


MediaPlayer *thePlayer;
MediaPlayer *voicePlayer;
OutputMixer *theMixer;
OutputMixer *theMixer2;

File myFile,voiceFile;
SDClass theSD;
bool ErrEnd = false;

extern bool songChanged;

enum
{
  Stop=0,
  PlayReady = 1,
  Playing =2,
  RecordReady =3,
  Recording =4,
  StopRecord =5,
  StatePrepare=6,
  PlayVoice = 7
};


static volatile int s_state = Stop;

#ifndef RGB_LCD
void changeColor(const int r, const int g ,const int b) {
    return;
}

void  plotWave(int val) {
  return;
}
#endif


/**
 * @brief Audio attention callback
 *
 * When audio internal error occurc, this function will be called back.
 */

static void mediarecorder_attention_cb(const ErrorAttentionParam *atprm)
{
  puts("Attention!");
  
  if (atprm->error_code >= AS_ATTENTION_CODE_WARNING)
    {
      ErrEnd = true;
   }
}

/**
 * @brief Audio attention callback
 *
 * When audio internal error occurc, this function will be called back.
 */

static void attention_cb(const ErrorAttentionParam *atprm)
{
  puts("Recorder Err!");
  
  if (atprm->error_code >= AS_ATTENTION_CODE_WARNING)
    {
      ErrEnd = true;
   }
}

/**
 * @brief Recorder done callback procedure
 *
 * @param [in] event        AsRecorderEvent type indicator
 * @param [in] result       Result
 * @param [in] sub_result   Sub result
 *
 * @return true on success, false otherwise
 */

static bool mediarecorder_done_callback(AsRecorderEvent event, uint32_t result, uint32_t sub_result)
{
    printf("MR cb %x %x %x\n", event, result, sub_result);

  return true;
}


/**
 * @brief Mixer done callback procedure
 *
 * @param [in] requester_dtq    MsgQueId type
 * @param [in] reply_of         MsgType type
 * @param [in,out] done_param   AsOutputMixDoneParam type pointer
 */
static void outputmixer_done_callback(MsgQueId requester_dtq,
                                      MsgType reply_of,
                                      AsOutputMixDoneParam *done_param)
{
  return;
}

/**
 * @brief Mixer data send callback procedure
 *
 * @param [in] identifier   Device identifier
 * @param [in] is_end       For normal request give false, for stop request give true
 */
static void outmixer_send_callback(int32_t identifier, bool is_end)
{
  AsRequestNextParam next;

  next.type = (!is_end) ? AsNextNormalRequest : AsNextStopResRequest;
  AS_RequestNextPlayerProcess(AS_PLAYER_ID_1, &next);

//  AS_RequestNextPlayerProcess(AS_PLAYER_ID_0, &next);

  /*
  if (s_state > Stop) {
    next.type = AsNextNormalRequest;
    AS_RequestNextPlayerProcess(AS_PLAYER_ID_1, &next);
  }
*/
  return;
}

/**
 * @brief Player done callback procedure
 *
 * @param [in] event        AsPlayerEvent type indicator
 * @param [in] result       Result
 * @param [in] sub_result   Sub result
 *
 * @return true on success, false otherwise
 */
static bool mediaplayer_done_callback(AsPlayerEvent event, uint32_t result, uint32_t sub_result)
{
  printf("mp cb %x %x %x\n", event, result, sub_result);
 // load next song
  return true;
}

static bool voiceplayer_done_callback(AsPlayerEvent event, uint32_t result, uint32_t sub_result)
{
  /* If result of "Play", restart recording (It should been stopped when "Play" requested) */

  return true;
}


/**
 * @brief Player decode callback procedure
 *
 * @param [in] pcm_param    AsPcmDataParam type
 */

short waveData[3];
void mediaplayer_decode_callback(AsPcmDataParam pcm_param)
{
    signed short *ptr = (signed short *)pcm_param.mh.getPa();

    for (unsigned int cnt = 0; cnt < pcm_param.size; cnt += 4)  {

        // Get a slice of wave data`
        if (cnt % 10 == 0) {
            waveData[0] = *ptr;
        }
        if (cnt % 20 == 0) {
            waveData[1] = *ptr;
        }
        if (cnt % 30 == 0) {
            waveData[2] = *ptr;
        }       
    }
 
  theMixer->sendData(OutputMixer0,
                     outmixer_send_callback,
                     pcm_param);
}

void signal_process(uint32_t size)
{

  printf("Size %d [%02x %02x %02x %02x %02x %02x %02x %02x ...]\n",
         size,
         s_buffer[0],
         s_buffer[1],
         s_buffer[2],
         s_buffer[3],
         s_buffer[4],
         s_buffer[5],
         s_buffer[6],
         s_buffer[7]);
}

/**
 * @brief clip function
 *
 * @param [in] val   source value
 * @param [in] peak  clip point value
 */
inline int16_t clip(int32_t val, int32_t peak)
{
  return (val > 0) ? ((val < peak) ? val : peak) : ((val > (-1 * peak)) ? val : (-1 * peak));
}

void pf_filter(AsPcmDataParam pcm_param)
{
  /* Example : RC filter for 16bit PCM */

  static const int PeakLevel = 32700;
  static const int LevelGain = 10;

  int16_t *ls = (int16_t*)pcm_param.mh.getPa();
  int16_t *rs = ls + 1;

  static int16_t ls_l = 0;
  static int16_t rs_l = 0;

  if (!ls_l && !rs_l)
    {
      ls_l = *ls * LevelGain;
      rs_l = *rs * LevelGain;
    }

  for (uint32_t cnt = 0; cnt < pcm_param.size; cnt += 4)
    {
      int32_t tmp;

      *ls = *ls * LevelGain;
      *rs = *rs * LevelGain;

      tmp = (ls_l * 98 / 100) + (*ls * 2 / 100);
      *ls = clip(tmp, PeakLevel);
      tmp = (rs_l * 98 / 100) + (*rs * 2 / 100);
      *rs = clip(tmp, PeakLevel);

      ls_l = *ls;
      rs_l = *rs;

      ls += 2;
      rs += 2;
    }
}

void voiceplayer_decode_callback(AsPcmDataParam pcm_param)
{
  /* You can imprement any audio signal process */
  {
      //  pf_filter(pcm_param); 
  }

  /* Output and sound audio data */

  theMixer->sendData(OutputMixer1,
                     outmixer_send_callback,
                     pcm_param);
}


bool  playerMode(char *fname) {
  myFile = theSD.open(fname);

  /* Verify file open */
  if (!myFile)
    {
      printf("File open error\n");
      return false;
    }
  printf("Open! %d\n", myFile);

  /* Send first frames to be decoded */
  err_t err = thePlayer->writeFrames(MediaPlayer::Player1, myFile);

  if (err != MEDIAPLAYER_ECODE_OK)
    {
      printf("File Read Error! =%d\n",err);
      myFile.close();
      return false;
    }
   

  // Start Player
  thePlayer->start(MediaPlayer::Player1, mediaplayer_decode_callback);
  return true;
}

bool playStream() {
 /* Send new frames to decode in a loop until file ends */
   err_t err = thePlayer->writeFrames(MediaPlayer::Player1, myFile);
   
 /*  Tell when player file ends */
  if (err == MEDIAPLAYER_ECODE_FILEEND) {
      printf("Main player File End!\n");
  }

  /* Show error code from player and stop */
  if (err) {
      printf("Main player error code: %d\n", err);
      goto stop_player;
  }
  
  if (songChanged || s_state == RecordReady) {
    songChanged = false;
    printf("Song changed");
    goto stop_player;
  }
  usleep(30000);
  return false;

stop_player:
  sleep(1);
  thePlayer->stop(MediaPlayer::Player1);
  myFile.close();
  /* return play end */
  return true;
}



err_t execute_aframe(uint32_t* size)
{
  err_t err = theRecorder->readFrames(s_buffer, buffer_size, size);

  if(((err == MEDIARECORDER_ECODE_OK) || (err == MEDIARECORDER_ECODE_INSUFFICIENT_BUFFER_AREA)) && (*size > 0)) 
    {
      signal_process(*size);
    }
  int ret = voiceFile.write((uint8_t*)&s_buffer, *size);
  if (ret < 0)
    {
      puts("File write error.");
      err = MEDIARECORDER_ECODE_FILEACCESS_ERROR;
    }

  return err;
}

void execute_frames()
{
  uint32_t read_size = 0;
  do
    {
      err_t err = execute_aframe(&read_size);
      if ((err != MEDIARECORDER_ECODE_OK)
       && (err != MEDIARECORDER_ECODE_INSUFFICIENT_BUFFER_AREA))
        {
          break;
        }
    }
  while (read_size > 0);
}

bool recorderMode(char *fname)
{
  /* Open file for data write on SD card */
  voiceFile = theSD.open(fname, FILE_WRITE);
  /* Verify file open */
  if (!voiceFile)
    {
      printf("Record file open error\n");
      ErrEnd =true;
    }
  else {
    theRecorder->start();
    puts("Recording Start!");
    return true;
  }
  return false;
}

/**
 * @brief Read audio data from the buffer
 */
bool recordStream()
{
    static int32_t total_size = 0;
    uint32_t read_size = 0;

  /* Execute audio data */
  err_t err = execute_aframe(&read_size);
  if (err != MEDIARECORDER_ECODE_OK && err != MEDIARECORDER_ECODE_INSUFFICIENT_BUFFER_AREA)
    {
      puts("Recording Error!");
      theRecorder->stop();
      goto exitRecording;      
    }
  else if (read_size>0)
    {
      total_size += read_size;
    }
  /* This sleep is adjusted by the time to write the audio stream file.
     Please adjust in according with the processing contents
     being processed at the same time by Application.
  */
   //usleep(10000);

  /* Stop Recording */
  if (total_size > (recoding_frames*288) || s_state == Stop)
    {
      theRecorder->stop();

      /* Get ramaining data(flushing) */
      sleep(1); /* For data pipline stop */
      execute_frames();
      
      goto exitRecording;
    }

  if (ErrEnd)
    {
      printf("Error End\n");
      theRecorder->stop();
      goto exitRecording;
    }

  return false;

exitRecording:

  voiceFile.close();

  theRecorder->deactivate();
  theRecorder->end();
  
  puts("End Recording");
  s_state = Stop;
  changeColor(0,0,0);
  sleep(1);
  /* return record end */
  return true;
}


/***************************************************************************
 * Simple shuffle song index
 * Place songs in SOUND directory with follwoing naming
 * music_XX.mp3  where XX is zero filled numgber
 * in sequencial order with no missing song number in between
 * 
 * music_00.mp3
 * music_01.mp3
 * ... etc
 * 
 ***************************************************************************/
#define MP3_DIR_NAME "MUSIC"
 
static short MAX_SONG_INDEX = 100;
short SongIndexes [100];

bool songChanged = false;

/* Random song index inside an array */
static void shuffle_songs(unsigned  seed) {
randomSeed(seed);

int byteSize = sizeof (short);
    int i,r;
    short src [MAX_SONG_INDEX];
    
    for (i = 0; i < MAX_SONG_INDEX; src [i] = ++i);
    
    for (i = MAX_SONG_INDEX; i > 0; i --)
    {  
        r = random (MAX_SONG_INDEX) % i;
        memcpy (&SongIndexes [MAX_SONG_INDEX - i], &src [r], byteSize);
        memcpy (&src [r], &src [i - 1], byteSize);
    }
    
    for (i = 0; i < MAX_SONG_INDEX; printf ("%d ", SongIndexes [i++] ) );
}

static short songIndex = 0;
static short voiceIndex = 0;
static void setSongIndex(  int value) {
  songIndex = abs(value + songIndex) % MAX_SONG_INDEX;
 // songIndex = abs(value) % MAX_SONG_INDEX;
 
  printf("song: %d\n", songIndex);
  
  songChanged = true;
}

int nextSongIndex() {
  songIndex ++;
  if (songIndex >= MAX_SONG_INDEX) {
    songIndex = 0;
  }
  return (int)SongIndexes[songIndex];
}

/* Reset total number of songs based on number of mp3 song files in the Music directory*/
void listSDFile() {
  File root = theSD.open("MUSIC");
  MAX_SONG_INDEX=countFilesInDir(root);
  printf("SONG Count: %d\n",MAX_SONG_INDEX);
}

int countFilesInDir(File dir) {
  int count = 0;
  while (true) {
    File entry = dir.openNextFile();
    if (!entry) {
      break;
    }
    if (entry.isDirectory()) {
       ; // skip
    } else {
      count++;
    }
    entry.close();
  }
  return count;
}

char fname[20];

char*  nextSongName() {
    snprintf(fname, sizeof(fname), "MUSIC/music_%02d.mp3", nextSongIndex());
    printf("%s", fname);
    return  fname;
}

char*  voiceFileName(short index) {
  if (index < 0)
       index = 0;
  snprintf(fname, sizeof(fname), "voice_%02d.mp3",index);
  printf("%s", fname);
  return  fname;
}

char*  nextVoiceName( ) {
  return voiceFileName(--voiceIndex);
}

char*  lastVoiceName( ) {
  return voiceFileName(voiceIndex-1);
}


/*****************************************************************************/

/**
 * @brief Setup Player and Mixer
 *
 * Set output device to Speakers/Headphones <br>
 * Initialize main player to decode stereo mp3 stream with 48 kb/s sample rate <br>
 * System directory "/mnt/sd0/BIN" will be searched for MP3 decoder (MP3DEC file)
 * Open "Sound.mp3" file <br>
 * Set volume to -16.0 dB
 */
bool jukeBoxMode = true;
void setup()
{

#ifdef ROTARY_ENCODER
  jukeBoxMode = isButtonDown();
  sleep (1);
  if( !jukeBoxMode && isButtonDown())
     jukeBoxMode = false;
#endif


#ifdef RGB_LCD
    LcdSetup();
#endif

    if (jukeBoxMode) {
        puts("MP3 Juke Box");

#ifdef SERVO
        ServoSetup();
#endif

        listSDFile();
        shuffle_songs(analogRead(A0));

#ifdef GPS_TIME
        GPSetup();
#endif
        
       
    }

#ifdef ROTARY_ENCODER
        IntRoterySetup();
#endif

    /* Enable Proper memory region based on usage as Jukebox or recorder */
    /* Following setting works in Media playback mode */
    /* Todo - Seqgmentation falut when trying to switch mode dynamically using the object level API */
    
     initMemoryPools();

     if (jukeBoxMode) {
         createStaticPools(MEM_LAYOUT_PLAYER);
     } 
     else {    
         createStaticPools(MEM_LAYOUT_RECORDER);
     }

    //createStaticPools(MEM_LAYOUT_RECORDINGPLAYER);
  
    /* start audio system */
    theRecorder = MediaRecorder::getInstance();
    //voicePlayer = MediaPlayer::getInstance();
    
    thePlayer = MediaPlayer::getInstance();
    theMixer  = OutputMixer::getInstance();
    //theMixer2  = OutputMixer::getInstance();

    theRecorder->begin(mediarecorder_attention_cb);
    thePlayer->begin();
    //voicePlayer->begin();
    
  
    /* Activate Baseband */
  
    theMixer->activateBaseband();
    //theMixer2->activateBaseband();
  
    /* Create Objects */
  
    thePlayer->create(MediaPlayer::Player1, attention_cb);
    //voicePlayer->create(MediaPlayer::Player0, attention_cb);
    theMixer->create(attention_cb);
    //theMixer2->activateBaseband();
  
    /* Set rendering clock */
    theRecorder->setCapturingClkMode(MEDIARECORDER_CAPCLK_NORMAL);

    theMixer->setRenderingClkMode(OUTPUTMIXER_RNDCLK_NORMAL);
    //theMixer2->setRenderingClkMode(OUTPUTMIXER_RNDCLK_NORMAL);
  
    /* Activate Objects. Set output device to Speakers/Headphones */
    theRecorder->activate(AS_SETRECDR_STS_INPUTDEVICE_MIC, mediarecorder_done_callback);
    //voicePlayer->activate(MediaPlayer::Player0, AS_SETPLAYER_OUTPUTDEVICE_SPHP, voiceplayer_done_callback);
     usleep(100 * 1000); /* waiting for Mic startup */
     
    thePlayer->activate(MediaPlayer::Player1, AS_SETPLAYER_OUTPUTDEVICE_SPHP, mediaplayer_done_callback);
   // theMixer2->activate(OutputMixer1, outputmixer_done_callback);
  
    theMixer->activate(OutputMixer0, outputmixer_done_callback);
     /* Main volume set to -16.0 dB, Main player and sub player set to 0 dB */
    theMixer->setVolume(0,0, 0);
  //  theMixer2->setVolume(-16,-8, 0);
  


  thePlayer->init(MediaPlayer::Player1, AS_CODECTYPE_MP3, "/mnt/sd0/BIN", AS_SAMPLINGRATE_AUTO, AS_CHANNEL_STEREO);
  //voicePlayer->init(MediaPlayer::Player0,AS_CODECTYPE_MP3, "/mnt/sd0/BIN", AS_SAMPLINGRATE_AUTO, AS_CHANNEL_STEREO);

  puts("players init");
  theRecorder->init(AS_CODECTYPE_MP3,
                    AS_CHANNEL_STEREO,
                    AS_SAMPLINGRATE_16000,
                    AS_BITLENGTH_16,
                    AS_BITRATE_48000,
                    "/mnt/sd0/BIN");

  puts("recorder init");

  s_state = (jukeBoxMode)? PlayReady: RecordReady;
}

/**
 * @brief Play audio frames until file ends
 */
void loop()
{
  if (ErrEnd)
    {
      printf("Error End\n");
      s_state == Stop;
      changeColor(100,0,0);
      ErrEnd = false;
    }

  /* This sleep is adjusted by the time to read the audio stream file.
     Please adjust in according with the processing contents
     being processed at the same time by Application.
  */

 switch (s_state)
    {
      case PlayReady:
        if (playerMode(nextSongName()))
            s_state = Playing;
        break;
        
      case Playing:
         plotWave(waveData[0]);
         if (playStream()) {
             // Song Ends
            Serial.println("Song Ends");
           s_state = PlayReady;       
         } 
        break;
      case PlayVoice:
        playerMode(lastVoiceName());
        s_state =Playing;
        break;
        
      case RecordReady:
        changeColor(0,0,0);
        if (recorderMode(nextVoiceName())) {
            s_state = Recording;
            changeColor(0,100,100);
        }
        break;
        
      case Recording:
       if (recordStream())
          {
            s_state = PlayVoice;
          }
          changeColor(0,100,0);
        break;
        
      default:
        break;
    }
    

 // usleep(1);

#ifdef RGB_LCD
  ServoLoop(); 
#endif 

#ifdef GPS_TIME
   GPSLoop();
#endif
 
 }

InterruptEncoder

C/C++
// Rotery Encoder for input using 2 interrupt driven pin
//https://playground.arduino.cc/Main/RotaryEncoders#Example5

#define encoderButton  PIN_D11
#define encoderPinA    PIN_D12
#define encoderPinB    PIN_D13

static volatile unsigned int encoderPos = 0;

void IntRoterySetup() {
  pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(encoderButton , INPUT);

  attachInterrupt(encoderPinA, doEncoderA, FALLING);
 // attachInterrupt(encoderPinB, doEncoderB, FALLING); //TODO: resolve issue with more than 2 interrupts.
  attachInterrupt(encoderButton, buttonCheck, FALLING);
  Serial.begin(115200);
}

 
void doEncoderA() {
  // look for a low-to-high on channel A
  if (digitalRead(encoderPinA) == HIGH) {

    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinB) == LOW) {
      encoderPos = encoderPos + 1;         // CW
    }
    else {
      encoderPos = encoderPos - 1;         // CCW
    }
  }

  else   // must be a high-to-low edge on channel A
  {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinB) == HIGH) {
      encoderPos = encoderPos + 1;          // CW
    }
    else {
      encoderPos = encoderPos - 1;          // CCW
    }
  }
 // Serial.println (encoderPos, DEC);
  // use for debugging - remember to comment out
  setSongIndex(encoderPos);
 
}

/*
void doEncoderB() {
  // look for a low-to-high on channel B
  if (digitalRead(encoderPinB) == HIGH) {

    // check channel A to see which way encoder is turning
    if (digitalRead(encoderPinA) == HIGH) {
      encoderPos = encoderPos + 1;         // CW
    }
    else {
      encoderPos = encoderPos - 1;         // CCW
    }
  }

  // Look for a high-to-low on channel B

  else {
    // check channel B to see which way encoder is turning
    if (digitalRead(encoderPinA) == LOW) {
      encoderPos = encoderPos + 1;          // CW
    }
    else {
      encoderPos = encoderPos - 1;          // CCW
    }
  }
     setSongIndex(encoderPos);

}
*/


bool isButtonDown() {
    return (digitalRead(encoderButton) == LOW);  
}

void buttonCheck() { 
  if ( s_state ==  Playing || s_state ==  Recording) {
        s_state = Stop;
        ledOff(PIN_LED0);
       
  } else if (encoderPos %2 == 1) {
    
     if ( s_state == Stop) {
        s_state = PlayReady;
        ledOff(PIN_LED0);
     }
  
  }  else  {
 
      Serial.println("Rec start");
      s_state == RecordReady;
      ledOn(PIN_LED0);
  } 
  usleep(20);
}
    
 
 

WaveOutput

C/C++
#include <Wire.h>
#include "rgb_lcd.h"

rgb_lcd lcd;

#ifdef RGB_LCD
void LcdSetup() 
{
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  // Print a message to the LCD.
  lcd.print("JUKE BOX");
}

void changeColor(const int r, const int g ,const int b) {
   lcd.setRGB(r,g,b);
}

/* Change the color based on db value here -8000 to 200 */
void  plotWave(int val) {
  // printf("%d,",val);
  if (val > 256) {
    lcd.setPWM(REG_RED,  map(val,256,8000,30,255));
  
  } else if (val > 100) {
    lcd.setPWM(REG_GREEN, val - 100);
  
  }  else if (val > 0) {
    lcd.setPWM(REG_BLUE, val);
    
  }  else
    lcd.setPWM(0,0);
}
#endif
/*******************   SERVO CONTROLLER      **************/
#include <Servo.h>

static Servo s_front; 
static Servo s_back;  
 
/**
 * @brief Initialize servo motor.
 */
void ServoSetup() {
  /* put your setup code here, to run once: */

  /*  Set pin that support PWM. */
  s_front.attach(PIN_D05);
  s_back.attach(PIN_D06);

  /* Set inital value of servo */
  s_front.write(20);
  s_front.write(20);

}

/**
 * Change servo motor angles's value in degrees here
 */
void ServoLoop() {
 if (s_state != Stop) {
   /* plot servo based on sampled wave data */
     if(waveData[1] > 0)
        s_front.write(  map(waveData[1] ,0,200, 30,160));

     if (waveData[2] <0)
         s_back.write( map(waveData[2] ,-4000,0, 30,160));

      delay(60);
 }
}

GPSTimer

C/C++
/*
 *  gnss.ino - GNSS example application
 *  Copyright 2018 Sony Semiconductor Solutions Corporation
 *
 *  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 St, Fifth Floor, Boston, MA  02110-1301  USA
 */

/**
 * @file gnss.ino
 * @author Sony Semiconductor Solutions Corporation
 * @brief GNSS example application
 * @details Spresense has an built in GNSS receiver which supports GPS and other
 *          GNSS satellites. This skecth provides an example the GNSS operation.
 *          Simply upload the sketch, reset the board and check the USB serial 
 *          output. After 3 seconds status information should start to appear.\n\n
 *
 *          This example code is in the public domain.
 */

/* include the GNSS library */
#include <GNSS.h>

#define STRING_BUFFER_SIZE  128       /**< %Buffer size */

#define RESTART_CYCLE       (60 * 5)  /**< positioning test term */

static SpGnss Gnss;                   /**< SpGnss object */
static void print_pos(SpNavData *pNavData);
static void print_condition(SpNavData *pNavData);

bool GpsCheck = true;

#include <math.h>

#define d2r (M_PI / 180.0)

//calculate haversine distance for linear distance
double haversine_km(double lat1, double long1, double lat2, double long2)
{
    double dlong = (long2 - long1) * d2r;
    double dlat = (lat2 - lat1) * d2r;
    double a = pow(sin(dlat/2.0), 2) + cos(lat1*d2r) * cos(lat2*d2r) * pow(sin(dlong/2.0), 2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = 6367 * c;

    return d;
}

double haversine_mi(double lat1, double long1, double lat2, double long2)
{
    double dlong = (long2 - long1) * d2r;
    double dlat = (lat2 - lat1) * d2r;
    double a = pow(sin(dlat/2.0), 2) + cos(lat1*d2r) * cos(lat2*d2r) * pow(sin(dlong/2.0), 2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    double d = 3956 * c; 

    return d;
}


/**
 * @brief Turn on / off the LED0 for CPU active notification.
 */
static void Led_isActive(void)
{
  static int state = 1;
  if (state == 1)
  {
    ledOn(PIN_LED0);
    state = 0;
  }
  else
  {
    ledOff(PIN_LED0);
    state = 1;
  }
}

/**
 * @brief Turn on / off the LED1 for positioning state notification.
 * 
 * @param [in] state Positioning state
 */
static void Led_isPosfix(bool state)
{
  if (state)
  {
    ledOn(PIN_LED1);
  }
  else
  {
    ledOff(PIN_LED1);
  }
}

/**
 * @brief Turn on / off the LED3 for error notification.
 * 
 * @param [in] state Error state
 */
static void Led_isError(bool state)
{
  if (state)
  {
    ledOn(PIN_LED3);
  }
  else
  {
    ledOff(PIN_LED3);
  }
}

/**
 * @brief Activate GNSS device and start positioning.
 */
void GPSetup() {
  /* put your setup code here, to run once: */

  int error_flag = 0;

  /* Set serial baudrate. */
  Serial.begin(115200);

  /* Wait HW initialization done. */
  sleep(3);

  /* Turn on all LED:Setup start. */
  ledOn(PIN_LED0);
  ledOn(PIN_LED1);
  ledOn(PIN_LED2);
  ledOn(PIN_LED3);

  /* Set Debug mode to Info */
 // Gnss.setDebugMode(PrintInfo);

  int result;

  /* Activate GNSS device */
  result = Gnss.begin();

  if (result != 0)
  {
    Serial.println("Gnss begin error!!");
    error_flag = 1;
  }
  else
  {
    /* Setup GNSS */
    Gnss.select(QZ_L1CA);  // Michibiki complement
    Gnss.select(QZ_L1S);   // Michibiki augmentation(Valid only in Japan)

    /* Start positioning */
    result = Gnss.start(COLD_START);
    if (result != 0)
    {
      Serial.println("Gnss start error!!");
      error_flag = 1;
    }
    else
    {
      Serial.println("Gnss setup OK");
    }
  }

  /* Turn off all LED:Setup done. */
  ledOff(PIN_LED0);
  ledOff(PIN_LED1);
  ledOff(PIN_LED2);
  ledOff(PIN_LED3);

  /* Set error LED. */
  if (error_flag == 1)
  {
    Led_isError(true);
    exit(0);
  }
}

/**
 * @brief %Print position information.
 */
static void print_pos(SpNavData *pNavData)
{
  static bool firstPass = true;
  char StringBuffer[STRING_BUFFER_SIZE];

  /* print time */
  snprintf(StringBuffer, STRING_BUFFER_SIZE, "%04d/%02d/%02d ", pNavData->time.year, pNavData->time.month, pNavData->time.day);
  Serial.print(StringBuffer);


  snprintf(StringBuffer, STRING_BUFFER_SIZE, "%02d:%02d:%02d.%06d, ", pNavData->time.hour, pNavData->time.minute, pNavData->time.sec, pNavData->time.usec);
  Serial.print(StringBuffer);

  /* print satellites count */
  snprintf(StringBuffer, STRING_BUFFER_SIZE, "numSat:%2d, ", pNavData->numSatellites);
  Serial.print(StringBuffer);

  /* print position data */
  if (pNavData->posFixMode == FixInvalid)
  {
    Serial.print("No-Fix, ");
  }
  else
  {
    Serial.print("Fix, ");
  }
  if (pNavData->posDataExist == 0)
  {
    Serial.print("No Position");
  }
  else
  {
    Serial.print("Lat=");
    Serial.print(pNavData->latitude, 6);
    Serial.print(", Lon=");
    Serial.print(pNavData->longitude, 6);

  }

    //Set Random Seed based on time obtaining satellite signal
    if (firstPass && pNavData->numSatellites >= 1) {
     shuffle_songs( pNavData->time.usec + pNavData->time.sec );
     setSongIndex(0);
     firstPass = false;
    }
  Serial.println("");


  // Disable GPS signal
  if (pNavData->time.year != 1980) {
    Serial.println("GPS Time acquired");
    // LCD Show Time
    GpsCheck = false;
  }
}

/**
 * @brief %Print satellite condition.
 */
static void print_condition(SpNavData *pNavData)
{
  char StringBuffer[STRING_BUFFER_SIZE];
  unsigned long cnt;

  /* Print satellite count. */
  snprintf(StringBuffer, STRING_BUFFER_SIZE, "numSatellites:%2d\n", pNavData->numSatellites);
  Serial.print(StringBuffer);

  for (cnt = 0; cnt < pNavData->numSatellites; cnt++)
  {
    const char *pType = "---";
    SpSatelliteType sattype = pNavData->getSatelliteType(cnt);

    /* Get satellite type. */
    /* Keep it to three letters. */
    switch (sattype)
    {
      case GPS:
        pType = "GPS";
        break;
      
      case GLONASS:
        pType = "GLN";
        break;

      case QZ_L1CA:
        pType = "QCA";
        break;

      case SBAS:
        pType = "SBA";
        break;

      case QZ_L1S:
        pType = "Q1S";
        break;

      default:
        pType = "UKN";
        break;
    }

    /* Get print conditions. */
    unsigned long Id  = pNavData->getSatelliteId(cnt);
    unsigned long Elv = pNavData->getSatelliteElevation(cnt);
    unsigned long Azm = pNavData->getSatelliteAzimuth(cnt);
    float sigLevel = pNavData->getSatelliteSignalLevel(cnt);

    /* Print satellite condition. */
    snprintf(StringBuffer, STRING_BUFFER_SIZE, "[%2d] Type:%s, Id:%2d, Elv:%2d, Azm:%3d, CN0:", cnt, pType, Id, Elv, Azm );
    Serial.print(StringBuffer);
    Serial.println(sigLevel, 6);
  }
}


void set_current_coord() {
   static unsigned long  srcElv;
   static unsigned long srcAzm;
}

/**
 * @brief %Print position information and satellite condition.
 * 
 * @details When the loop count reaches the RESTART_CYCLE value, GNSS device is 
 *          restarted.
 */
void GPSLoop()
{
  /* put your main code here, to run repeatedly: */
  
  static int LoopCount = 0;
  static int LastPrintMin = 0;

  if (!GpsCheck)
    return;
  /* Blink LED. */
  Led_isActive();

  /* Check update. */
  if (Gnss.waitUpdate(-1))
  {
    /* Get NaviData. */
    SpNavData NavData;
    Gnss.getNavData(&NavData);

    /* Set posfix LED. */
    bool LedSet = (NavData.posDataExist && (NavData.posFixMode != FixInvalid));
    Led_isPosfix(LedSet);

    /* Print satellite information every minute. */
    if (NavData.time.minute != LastPrintMin)
    {
      print_condition(&NavData);
      LastPrintMin = NavData.time.minute;
    }
    
    /* Print position information. */
    print_pos(&NavData);
  }
  else
  {
    /* Not update. */
    Serial.println("data not update");
  }
  

  /* Check loop count. */
  LoopCount++;
  if (LoopCount >= RESTART_CYCLE)
  {
    int error_flag = 0;

    /* Turn off LED0 */
    ledOff(PIN_LED0);

    /* Set posfix LED. */
    Led_isPosfix(false);

    /* Restart GNSS. */
    if (Gnss.stop() != 0)
    {
      Serial.println("Gnss stop error!!");
      error_flag = 1;
    }
    else if (Gnss.end() != 0)
    {
      Serial.println("Gnss end error!!");
      error_flag = 1;
    }
    else
    {
      Serial.println("Gnss stop OK.");
    }

    if (Gnss.begin() != 0)
    {
      Serial.println("Gnss begin error!!");
      error_flag = 1;
    }
    else if (Gnss.start(HOT_START) != 0)
    {
      Serial.println("Gnss start error!!");
      error_flag = 1;
    }
    else
    {
      Serial.println("Gnss restart OK.");
    }

    LoopCount = 0;

    /* Set error LED. */
    if (error_flag == 1)
    {
      Led_isError(true);
      exit(0);
    }
  }
}

Credits

Leon Chu

Leon Chu

11 projects • 10 followers
Indie Developer / Artist / Lifelong learner

Comments