static msg_t stream_acc_thread(void *arg) {
  uint16_t period = *(uint16_t *)arg;
  systime_t time = chTimeNow();

  chRegSetThreadName("lsm303_stream_acc");

  while (TRUE) {
    chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6u %5d %5d %5d\r\n", (uint32_t)time,
             acc_data.x, acc_data.y, acc_data.z);
    time += MS2ST(period);
    chThdSleepUntil(time);
  }
  return 0;
}

static void cmd_setvolume(Stream *chp, int argc, char *argv[]) {
  uint8_t vol=0;
  if (argc==0)
     vol= GET_BYTE_PARAM(TRX_VOLUME);
  else {
     if (argc > 0) 
        sscanf(argv[0], "%hhu", &vol);
     if (vol>8) vol=8;
  
     SET_BYTE_PARAM(TRX_VOLUME, vol);
     radio_setVolume(vol);
  }
  chprintf(chp, "VOLUME: %d\r\n", vol);
}

static void cmd_stream_gyro(BaseSequentialStream*chp, int argc, char *argv[]) {
  Thread *tp;

  if (argc != 1) {
    chprintf(chp, "Usage: sg <Hz>\r\n");
    return;
  }

  period = (1000 / atoi(argv[0]));

  if (gyrotp == NULL)
    gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);

  tp = chThdCreateFromHeap(NULL, WA_SIZE_256B, chThdGetPriority(),
                           stream_gyro_thread, (void *)&period);

  if (tp == NULL) {
    chprintf(chp, "out of memory\r\n");
    return;
  }

  return;
}

static void cmd_setsquelch(Stream *chp, int argc, char *argv[]) {
   uint8_t sq=0;
   if (argc == 0)
      sq = GET_BYTE_PARAM(TRX_SQUELCH);
   else {
      if (argc > 0) 
         sscanf(argv[0], "%hhu", &sq);
      if (sq>8) sq=8; 
      
      SET_BYTE_PARAM(TRX_SQUELCH, sq);
      radio_setSquelch(sq);
   }
   chprintf(chp, "SQUELCH: %d\r\n", sq); 
}

void datalog(void)
{
		float nick, roll, yaw;
	uint32_t system_time;
	 		//update_IMU();	 //Ersetzen durch Interrupt Handler!!!!!!
			nick = getEuler_nick();
			roll = getEuler_roll();
			yaw = getEuler_yaw();
		
		if(Datalogger_ready() && !datalog_main_opened)
		{
				//rc_main = f_mkfs(0,0,0);
				rc_main = f_open(&Fil_Main, "QuadMain.TXT", FA_WRITE | FA_CREATE_ALWAYS);
				if(rc_main != FR_OK)
				{
					chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: f_open() failed %d\r\n", rc_main);
					return;
				}	
				//rc_main = f_printf(&Fil, "moin\r\n");	 
				rc_main = f_sync(&Fil_Main);
				if(rc_main != FR_OK)
				{
					chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: f_sync() failed %d\r\n", rc_main);
					return;
				}	
				chprintf((BaseChannel *) &SD2, "SD QuadMain.TXT: opened successfull\r\n");
				f_printf(&Fil_Main, "Time_Main; Nick_Main; Roll_Main; Yaw_Main\r\n");
				f_sync(&Fil_Main);
				datalog_main_opened = TRUE;
		}
		if(Datalogger_ready() && datalog_main_opened)
		{
			system_time = chTimeNow();
			f_printf(&Fil_Main, "%d;%d;%d;%d\r\n",system_time,(int)(nick*100),(int)(roll*100),(int)(yaw*100));
			rc_main = f_sync(&Fil_Main);
		}
}

static THD_FUNCTION(logThread,arg) {
  UNUSED(arg);
  while(TRUE){
    
    //  int i =0;
    while (isLogging && counter < 3500) {

      // for( i; i<(sizeof(press_ft)); i++){
      double press = get_pressure();
      chprintf((BaseSequentialStream*)&SD1, "1. %04d\n\r ", (int)press);//time stamp.

      //=(1-(A18/1013.25)^0.190284)*145366.45
      double altitude_ft = (1-((press/1013.25),0.190284))*145366.45;//took out "pow"
      //converts to Altitude measurement

      //converts above value to int for printing purposes
      int final_ft = (int) altitude_ft;
	
      chprintf((BaseSequentialStream*)&SD1, "2.  %04d\n\r ", final_ft);//time stamp.

      press_ft[counter]= final_ft;
      // int final_m = (int)final_ft/3.280839895;
      //converts to meters, then to an int for printing purposes. 
      int final_m = (int) final_ft/3.280839895;

      chprintf((BaseSequentialStream*)&SD1, "3.  %04d\n\r ",final_m);//time stamp.
      chprintf((BaseSequentialStream*)&SD1, "array.  %04d\n\r ",press_ft[counter]);//aray value
      chprintf((BaseSequentialStream*)&SD1, "iterator.  %04d\n\r ",counter);//i.

      counter++;
      chThdSleepMilliseconds(1); //escape for scheduler.
      //  chThdSleepMilliseconds(1); //escape for scheduler.
    }
    chThdSleepMilliseconds(1); //escape for scheduler.
  }
  return 0;
}

void cmd_pwr_cfg( BaseChannel *chp, int argc, char * argv [] )
{
	(void)chp;
	if ( argc > 0 )
	{
		chMtxLock( &g_mutex );
		int res = atoi( argv[0] );
		g_firstOnDelay = res;
		if ( argc > 1 )
		{
			res = atoi( argv[1] );
			g_onDelay = res;
			if ( argc > 2 )
			{
				res = atoi( argv[2] );
				g_offDelay = res;
			}
		}
		chMtxUnlock();
        chprintf( chp, "ok:pwrcfg" );
	}
    else
        chprintf( chp, "err:pwrcfg" );
}

/**
 * @brief   Reads a whole line from the input channel.
 * @note    Input chars are echoed on the same stream object with the
 *          following exceptions:
 *          - DEL and BS are echoed as BS-SPACE-BS.
 *          - CR is echoed as CR-LF.
 *          - 0x4 is echoed as "^D".
 *          - Other values below 0x20 are not echoed.
 *          .
 *
 * @param[in] chp       pointer to a @p BaseSequentialStream object
 * @param[in] line      pointer to the line buffer
 * @param[in] size      buffer maximum length
 * @return              The operation status.
 * @retval true         the channel was reset or CTRL-D pressed.
 * @retval false        operation successful.
 *
 * @api
 */
bool shellGetLine(BaseSequentialStream *chp, char *line, unsigned size) {
  char *p = line;

  while (true) {
    char c;

    if (streamRead(chp, (uint8_t *)&c, 1) == 0)
      return true;
#if (SHELL_CMD_EXIT_ENABLED == TRUE) && !defined(_CHIBIOS_NIL_)
    if (c == 4) {
      chprintf(chp, "^D");
      return true;
    }
#endif
    if ((c == 8) || (c == 127)) {
      if (p != line) {
        streamPut(chp, c);
        streamPut(chp, 0x20);
        streamPut(chp, c);
        p--;
      }
      continue;
    }
    if (c == '\r') {
      chprintf(chp, "\r\n");
      *p = 0;
      return false;
    }
    if (c < 0x20)
      continue;
    if (p < line + size - 1) {
      streamPut(chp, c);
      *p++ = (char)c;
    }
  }
}

void setUltrasonicAddress(BaseSequentialStream *chp, int oldAddress, int newAddress) {
    msg_t retCode = RDY_OK;

    // Reprogramming sequence is: 0xA0, 0xAA, 0xA5, newAddress.
    retCode &= commandUltrasonic(oldAddress, 0xA0);
    chThdSleepMilliseconds(50);
    chprintf(chp, ".");

    retCode &= commandUltrasonic(oldAddress, 0xAA);
    chThdSleepMilliseconds(50);
    chprintf(chp, ".");

    retCode &= commandUltrasonic(oldAddress, 0xA5);
    chThdSleepMilliseconds(50);
    chprintf(chp, ".");

    retCode &= commandUltrasonic(oldAddress, newAddress);
    chThdSleepMilliseconds(50);

    if (retCode == RDY_OK)
        chprintf(chp, " Success.\r\n");
    else
        chprintf(chp, " Failed.\r\n");
}

static void
cmd_threads(BaseSequentialStream *chp, int argc, char *argv[])
{
   static const char *states[] = {CH_STATE_NAMES};
   thread_t *tp;

   (void)argv;
   if (argc > 0)
   {
      chprintf(chp, "Usage: threads\r\n");
      return;
   }

   chprintf(chp, "    addr    stack prio refs     state\r\n");
   tp = chRegFirstThread();
   do
   {
      chprintf(chp, "%08lx %08lx %4lu %4lu %9s\r\n",
            (uint32_t)tp, (uint32_t)tp->p_ctx.r13,
            (uint32_t)tp->p_prio, (uint32_t)(tp->p_refs - 1),
            states[tp->p_state]);
      tp = chRegNextThread(tp);
   } while (tp != NULL);
}

static msg_t Thread1(void *arg) {

  BaseSequentialStream *serp = (BaseSequentialStream *) &SD1;
  thread1 = chThdSelf();
  while (TRUE) {
    chEvtWaitAny((eventmask_t) 1);
    chprintf(serp, "WIDTH[%lu ms, %u ticks] PERIOD[%lu ms, %u ticks]\r\n",
                   ((uint32_t) width * 1000) / ICU_CLK,
                   width,
                   ((uint32_t) period * 1000) / ICU_CLK,
                   period);
    chEvtSignal(thread_main, (eventmask_t) 1);
  }
  return 0;
}

/******************************************************************************
 * Function name:	cmd_stop
 *
 * Description:		Displays/modifies PID feedback coefficients
 *
 * Arguments:		l p: Displays lateral proportional coefficient
 * 					l i: Displays lateral integral coefficient
 * 					l d: Displays lateral derivative coefficient
 *
 * 					v p: Displays vertical proportional coefficient
 * 					v i: Displays vertical integral coefficient
 * 					v d: Displays vertical derivative coefficient
 *
 * 					l p #: Sets lateral proportional coefficient to #
 * 					l i #: Sets lateral integral coefficient to #
 * 					l d #: Sets lateral derivative coefficient to #
 *
 * 					v p #: Sets vertical proportional coefficient to #
 * 					v i #: Sets vertical integral coefficient to #
 * 					v d #: Sets vertical derivative coefficient to #
 *
 *****************************************************************************/
static void cmd_gain(BaseSequentialStream *chp, int argc, char *argv[]) {

CONTROL_AXIS_STRUCT *axis_p = NULL;

	(void)argv;
	if (argc > 0) {

		U8ShellEnable = 0;
		chprintf(chp, "System Disabled. Type 'enable' to resume. \r\n");

		if (*argv[0] == 'V' || *argv[0] == 'v') {
			axis_p = &vertAxisStruct;
			chprintf(chp, "Vertical Axis ");
		}
		else if(*argv[0] == 'L' || *argv[0] == 'l') {
			axis_p = &latAxisStruct;
			chprintf(chp, "Lateral Axis ");
		}

		if (*argv[1] == 'P' || *argv[1] == 'p') {
			if(argv[2])
				axis_p->U16PositionPGain = atoi(argv[2]);
			chprintf(chp, "P Gain : %d \r\n", axis_p->U16PositionPGain);
		}
		else if(*argv[1] == 'I' || *argv[1] == 'i') {
			if(argv[2])
				axis_p->U16PositionIGain = atoi(argv[2]);
			chprintf(chp, "I Gain : %d \r\n", axis_p->U16PositionIGain);
		}
		else if(*argv[1] == 'D' || *argv[1] == 'd') {
			if(argv[2])
				axis_p->U16PositionDGain = atoi(argv[2]);
			chprintf(chp, "D Gain : %d \r\n", axis_p->U16PositionDGain);
		}
		else {
			chprintf(chp, "\n\tP Gain : %d \r\n", axis_p->U16PositionPGain);
			chprintf(chp, "\tI Gain : %d \r\n", axis_p->U16PositionIGain);
			chprintf(chp, "\tD Gain : %d \r\n", axis_p->U16PositionDGain);
		}
	}
}

/*
 * Stream thread
 */
static msg_t stream_raw_thread(void *arg) {
  uint16_t period = *(uint16_t *)arg;
  systime_t time = chTimeNow();

  while (TRUE) {
    chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f %f %f %f %d %d %d\r\n", (int)time,
            gyro_data.x / 57.143, gyro_data.y / 57.143, gyro_data.z / 57.143,
            acc_data.x / 1000.0, acc_data.y / 1000.0, acc_data.z / 1000.0,
            mag_data.x, mag_data.y, mag_data.z);
    time += MS2ST(period);
    chThdSleepUntil(time);
  }

  return 0;
}

static void data_udp_rx_serve(struct netconn *conn) {
	BaseSequentialStream *chp = getActiveUsbSerialStream();

	static uint8_t       count  = 0;

	struct netbuf        *inbuf;

	char                 *buf;

	uint16_t             buflen = 0;
	uint16_t             i      = 0;

	err_t                err;

	/*
	 * Read the data from the port, blocking if nothing yet there.
	 * We assume the request (the part we care about) is in one netbuf
	 */
	err = netconn_recv(conn, &inbuf);
	if (err == ERR_OK) {
		netbuf_data(inbuf, (void **)&buf, &buflen);
		palClearPad(TIMEINPUT_PORT, TIMEINPUT_PIN);     // negative pulse for input.
		chprintf(chp, "\r\nsensor rx (from FC): %d ", count++);
		palSetPad(TIMEINPUT_PORT, TIMEINPUT_PIN);
		for(i=0; i<buflen; ++i) {
			chprintf(chp, "%c", buf[i]);
		}
		chprintf(chp, "\r\n");
	}
	netconn_close(conn);

	/* Delete the buffer (netconn_recv gives us ownership,
	 * so we have to make sure to deallocate the buffer)
	 */
	netbuf_delete(inbuf);
}

static void cmd_threads(Stream *chp, int argc, char *argv[]) {
  static const char *states[] = {CH_STATE_NAMES};
  thread_t *tp;
  
  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: threads\r\n");
    return;
  }
  chprintf(chp, "stklimit    stack     addr refs prio     state  name\r\n\r\n");
  tp = chRegFirstThread();
  do {
    #if (CH_DBG_ENABLE_STACK_CHECK == TRUE) || (CH_CFG_USE_DYNAMIC == TRUE)
    uint32_t stklimit = (uint32_t)tp->wabase;
    #else
    uint32_t stklimit = 0U;
    #endif
    chprintf(chp, "%08lx %08lx %08lx %4lu %4lu %9s  %s\r\n",
             stklimit, (uint32_t)tp->ctx.sp, (uint32_t)tp, 
             (uint32_t)tp->refs - 1, (uint32_t)tp->prio, states[tp->state],
             tp->name == NULL ? "" : tp->name);
    tp = chRegNextThread(tp);
  } while (tp != NULL);
}

void cmd_phy(BaseSequentialStream *chp, int argc, char *argv[]) {
	uint32_t phy_val     = 0;
	uint32_t reg_to_ping = 0;

	//uint32_t bmcr_val = 0;

	if (argc != 1) {
		chprintf(chp, "Usage: phy reg(decimal)\r\n");
		return;
	}

//	bmcr_val = mii_read(&ETHD1, MII_BMCR);
//
//	mii_write(&ETHD1, MII_BMCR, (bmcr_val & ~(1<<12)) );
//
//	bmcr_val = mii_read(&ETHD1, MII_BMCR);
//
//	mii_write(&ETHD1, 0x1f,( bmcr_val | 1<<13));

	reg_to_ping = atoi(argv[0]);
	phy_val = mii_read(&ETHD1, reg_to_ping);
	chprintf(chp, "phy reg 0x%x value:\t0x%x\n\r", reg_to_ping, phy_val);

}

/* Send a command to the CS43L22 through I2C */
static msg_t _cs43l22_set( u8 reg , u8 value )
{
	u8 txBuffer[ 2 ];
	txBuffer[0] = reg;
	txBuffer[1] = value;
	msg_t rv = i2cMasterTransmitTimeout(
			& I2S_OI2C_DRIVER , I2S_OI2C_ADDRESS ,
			txBuffer , 2 ,
			NULL , 0 ,
			I2S_OI2C_TIMEOUT );
	if ( rv ) {
		chprintf( (void*)&SD2 ,
				"I2C 0x%0.2x <- 0x%0.2x ERROR %d\r\n" ,
				reg , value , rv );
		chprintf( (void*)&SD2 ,
				"    status = 0x%x\r\n" ,
				i2cGetErrors( & I2S_OI2C_DRIVER ) );
	} else {
		chprintf( (void*)&SD2 ,
				"I2C 0x%0.2x <- 0x%0.2x OK\r\n" ,
				reg , value );
	}
	return rv;
}

//-----------------------------------------------------------------------------
static void
cmd_write(BaseSequentialStream * chp, int argc, char * argv[])
{
    (void)argc;
    (void)argv;

    factory_config.preamble = FACTORY_CONFIG_MAGIC;
    factory_config.version = FACTORY_CONFIG_VERSION;
    factory_config.checksum = calc_checksum_16((uint8_t*)&factory_config,
                              sizeof(factory_config)-sizeof(factory_config.checksum));

    uint8_t * eeprombuf = (uint8_t*) &factory_config;
    spiEepromWriteBytes(&spiEepromD1, FACTORY_CONFIG_ADDRESS, eeprombuf, FACTORY_CONFIG_SIZE);

    chprintf(chp, "Config written\n");
}

static void cmd_pwm(BaseSequentialStream *chp, int argc, char *argv[]) {

  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: p\r\n");
    return;
  }

  pwm_cycles = 8;

//  palClearPad(MAX3232_GPIO, MAX3232_EN);
//  chThdSleepMilliseconds(10);

  pwmEnableChannel(&PWM_DRIVER, 2, 25);
  pwmEnableChannel(&PWM_DRIVER, 3, 25);
}

int main(void) {
	halInit();
	chSysInit();

	gdispInit();
	gdispClear(Lime);

	gfxConsoleInit(&CON1, 0, 0, gdispGetWidth(), gdispGetHeight(), &fontLarger, Black, White);
	chprintf((BaseSequentialStream *)&CON1, "Hello the time is %d\nGoodbye.", chTimeNow());


	while (TRUE) {
		
		chThdSleepMilliseconds(100);
	}
}