/* Monitor the power supply, if it is below 3.3 
 *turn off LED which is on the blimp*/
//SVSMHRRL=SVSh_it+, SVMh SVSMHACE = 0,SVMHFP=0;

#include "power_monitor.h"

void pmm_init() {
	// Open PMM module registers for write access		
    PMMCTL0_H = PMMPW_H;
	// Set VCore to new level
	PMMCTL0_L = PMMCOREV_3;

	// Wait till new level reached
	if ((PMMIFG & SVMLIFG)){
	    while ((PMMIFG & SVMLVLRIFG) == 0);
	} 

    SVSMHCTL = SVMHE + SVSMHRRL_6;// SVM high-side enable, threshod around3.1V
	while(!(PMMIFG & SVSMHDLYIFG));
	//Wait till SUM is settled
    PMMRIE = SVMHIE+ SVMHVLRIE;// SVM high-side and reached interrupt enabled.
    PMMCTL0_H = 0x00;  // Close PMM module registers for write access
}

 #pragma vector=SYSNMI_VECTOR
__interrupt void SYSNMI_ISR(void)
{   
	switch(SYSSNIV)
    {
        case SYSSNIV_SVMHIFG:
		    PMMCTL0_H = PMMPW_H;
            PMMRIE &= ~SVMHIE;// disable SVMH interrupt 
            //TODO:send back the power is low
			led_set(LED_RED,LED_ON);
			__delay_cycles(8300);
            PMMIFG &= ~SVMHIFG;
            PMMRIE |= SVMHIE; 
            PMMCTL0_H = 0x00;
			break;
		case SYSSNIV_VLRHIFG:
           	led_set(LED_RED,LED_OFF);
		     break;	 
        default:
           break;
    }
}