int mbox_recv(struct mbox_chan *chan, void *data, ulong timeout_us)
{
struct mbox_ops *ops = mbox_dev_ops(chan->dev);
ulong start_time;
int ret;
debug("%s(chan=%p, data=%p, timeout_us=%ld)\n", __func__, chan, data,
timeout_us);
start_time = timer_get_us();
/*
* Account for partial us ticks, but if timeout_us is 0, ensure we
* still don't wait at all.
*/
if (timeout_us)
timeout_us++;
for (;;) {
ret = ops->recv(chan, data);
if (ret != -ENODATA)
return ret;
if ((timer_get_us() - start_time) >= timeout_us)
return -ETIMEDOUT;
}
}
static int rsb_await_trans(void)
{
struct sunxi_rsb_reg * const rsb =
(struct sunxi_rsb_reg *)SUNXI_RSB_BASE;
unsigned long tmo = timer_get_us() + 1000000;
u32 stat;
int ret;
while (1) {
stat = readl(&rsb->stat);
if (stat & RSB_STAT_LBSY_INT) {
ret = -EBUSY;
break;
}
if (stat & RSB_STAT_TERR_INT) {
ret = -EIO;
break;
}
if (stat & RSB_STAT_TOVER_INT) {
ret = 0;
break;
}
if (timer_get_us() > tmo) {
ret = -ETIME;
break;
}
}
writel(stat, &rsb->stat); /* Clear status bits */
return ret;
}
static int sunxi_hdmi_hpd_detect(int hpd_delay)
{
struct sunxi_ccm_reg * const ccm =
(struct sunxi_ccm_reg *)SUNXI_CCM_BASE;
struct sunxi_hdmi_reg * const hdmi =
(struct sunxi_hdmi_reg *)SUNXI_HDMI_BASE;
unsigned long tmo = timer_get_us() + hpd_delay * 1000;
/* Set pll3 to 300MHz */
clock_set_pll3(300000000);
/* Set hdmi parent to pll3 */
clrsetbits_le32(&ccm->hdmi_clk_cfg, CCM_HDMI_CTRL_PLL_MASK,
CCM_HDMI_CTRL_PLL3);
/* Set ahb gating to pass */
#ifdef CONFIG_SUNXI_GEN_SUN6I
setbits_le32(&ccm->ahb_reset1_cfg, 1 << AHB_RESET_OFFSET_HDMI);
#endif
setbits_le32(&ccm->ahb_gate1, 1 << AHB_GATE_OFFSET_HDMI);
/* Clock on */
setbits_le32(&ccm->hdmi_clk_cfg, CCM_HDMI_CTRL_GATE);
writel(SUNXI_HDMI_CTRL_ENABLE, &hdmi->ctrl);
writel(SUNXI_HDMI_PAD_CTRL0_HDP, &hdmi->pad_ctrl0);
while (timer_get_us() < tmo) {
if (readl(&hdmi->hpd) & SUNXI_HDMI_HPD_DETECT)
return 1;
}
return 0;
}
/*
* Wait up to 1s for value to be set in given part of reg.
*/
void mctl_await_completion(u32 *reg, u32 mask, u32 val)
{
unsigned long tmo = timer_get_us() + 1000000;
while ((readl(reg) & mask) != val) {
if (timer_get_us() > tmo)
panic("Timeout initialising DRAM\n");
}
}
static void sunxi_dw_hdmi_phy_init(void)
{
struct sunxi_hdmi_phy * const phy =
(struct sunxi_hdmi_phy *)(SUNXI_HDMI_BASE + HDMI_PHY_OFFS);
unsigned long tmo;
u32 tmp;
/*
* HDMI PHY settings are taken as-is from Allwinner BSP code.
* There is no documentation.
*/
writel(0, &phy->ctrl);
setbits_le32(&phy->ctrl, BIT(0));
udelay(5);
setbits_le32(&phy->ctrl, BIT(16));
setbits_le32(&phy->ctrl, BIT(1));
udelay(10);
setbits_le32(&phy->ctrl, BIT(2));
udelay(5);
setbits_le32(&phy->ctrl, BIT(3));
udelay(40);
setbits_le32(&phy->ctrl, BIT(19));
udelay(100);
setbits_le32(&phy->ctrl, BIT(18));
setbits_le32(&phy->ctrl, 7 << 4);
/* Note that Allwinner code doesn't fail in case of timeout */
tmo = timer_get_us() + 2000;
while ((readl(&phy->status) & 0x80) == 0) {
if (timer_get_us() > tmo) {
printf("Warning: HDMI PHY init timeout!\n");
break;
}
}
setbits_le32(&phy->ctrl, 0xf << 8);
setbits_le32(&phy->ctrl, BIT(7));
writel(0x39dc5040, &phy->pll);
writel(0x80084343, &phy->clk);
udelay(10000);
writel(1, &phy->unk3);
setbits_le32(&phy->pll, BIT(25));
udelay(100000);
tmp = (readl(&phy->status) & 0x1f800) >> 11;
setbits_le32(&phy->pll, BIT(31) | BIT(30));
setbits_le32(&phy->pll, tmp);
writel(0x01FF0F7F, &phy->ctrl);
writel(0x80639000, &phy->unk1);
writel(0x0F81C405, &phy->unk2);
/* enable read access to HDMI controller */
writel(0x54524545, &phy->read_en);
/* descramble register offsets */
writel(0x42494E47, &phy->unscramble);
}
static int tegra186_bpmp_probe(struct udevice *dev)
{
struct tegra186_bpmp *priv = dev_get_priv(dev);
int ret;
ulong tx_base, rx_base, start_time;
debug("%s(dev=%p) (priv=%p)\n", __func__, dev, priv);
ret = mbox_get_by_index(dev, 0, &priv->mbox);
if (ret) {
error("mbox_get_by_index() failed: %d\n", ret);
return ret;
}
tx_base = tegra186_bpmp_get_shmem(dev, 0);
if (IS_ERR_VALUE(tx_base)) {
error("tegra186_bpmp_get_shmem failed for tx_base\n");
return tx_base;
}
rx_base = tegra186_bpmp_get_shmem(dev, 1);
if (IS_ERR_VALUE(rx_base)) {
error("tegra186_bpmp_get_shmem failed for rx_base\n");
return rx_base;
}
debug("shmem: rx=%lx, tx=%lx\n", rx_base, tx_base);
ret = tegra_ivc_init(&priv->ivc, rx_base, tx_base, BPMP_IVC_FRAME_COUNT,
BPMP_IVC_FRAME_SIZE, tegra186_bpmp_ivc_notify);
if (ret) {
error("tegra_ivc_init() failed: %d\n", ret);
return ret;
}
tegra_ivc_channel_reset(&priv->ivc);
start_time = timer_get_us();
for (;;) {
ret = tegra_ivc_channel_notified(&priv->ivc);
if (!ret)
break;
/* Timeout 100ms */
if ((timer_get_us() - start_time) > 100 * 1000) {
error("Initial IVC reset timed out (%d)\n", ret);
ret = -ETIMEDOUT;
goto err_free_mbox;
}
}
return 0;
err_free_mbox:
mbox_free(&priv->mbox);
return ret;
}
/*
* Wait up to 200ms for value to be set in given part of reg.
*/
static int await_completion(u32 *reg, u32 mask, u32 val)
{
unsigned long tmo = timer_get_us() + 200000;
while ((readl(reg) & mask) != val) {
if (timer_get_us() > tmo) {
printf("DDC: timeout reading EDID\n");
return -ETIME;
}
}
return 0;
}
int
nand_wait_timeout(u32 *reg, u32 mask, u32 val)
{
unsigned long tmo = timer_get_us() + 1000000; /* 1s */
while ((readl(reg) & mask) != val) {
if (timer_get_us() > tmo)
return -ETIMEDOUT;
}
return 0;
}
/* delay x useconds */
void __udelay(unsigned long usec)
{
long tmo = usec * (TIMER_CLK / 1000) / 1000;
unsigned long now, last = timer_get_us();
while (tmo > 0) {
now = timer_get_us();
if (last > now) /* count up timer overflow */
tmo -= TIMER_LOAD_VAL - last + now;
else
tmo -= now - last;
last = now;
}
}
/**
* Perform the next stage of the LCD init if it is time to do so.
*
* LCD init can be time-consuming because of the number of delays we need
* while waiting for the backlight power supply, etc. This function can
* be called at various times during U-Boot operation to advance the
* initialization of the LCD to the next stage if sufficient time has
* passed since the last stage. It keeps track of what stage it is up to
* and the time that it is permitted to move to the next stage.
*
* The final call should have wait=1 to complete the init.
*
* @param blob fdt blob containing LCD information
* @param wait 1 to wait until all init is complete, and then return
* 0 to return immediately, potentially doing nothing if it is
* not yet time for the next init.
*/
static int tegra_lcd_check_next_stage(const void *blob,
struct tegra_lcd_priv *priv, int wait)
{
if (priv->stage == STAGE_DONE)
return 0;
do {
/* wait if we need to */
debug("%s: stage %d\n", __func__, priv->stage);
if (priv->stage != STAGE_START) {
int delay = priv->timer_next - timer_get_us();
if (delay > 0) {
if (wait)
udelay(delay);
else
return 0;
}
}
if (handle_stage(blob, priv))
return -1;
} while (wait && priv->stage != STAGE_DONE);
if (priv->stage == STAGE_DONE)
debug("%s: LCD init complete\n", __func__);
return 0;
}
static int rockchip_spi_probe(struct udevice *bus)
{
struct rockchip_spi_platdata *plat = dev_get_platdata(bus);
struct rockchip_spi_priv *priv = dev_get_priv(bus);
int ret;
debug("%s: probe\n", __func__);
priv->regs = (struct rockchip_spi *)plat->base;
priv->last_transaction_us = timer_get_us();
priv->max_freq = plat->frequency;
/*
* Use 99 MHz as our clock since it divides nicely into 594 MHz which
* is the assumed speed for CLK_GENERAL.
*/
ret = clk_set_rate(&priv->clk, 99000000);
if (ret < 0) {
debug("%s: Failed to set clock: %d\n", __func__, ret);
return ret;
}
priv->input_rate = ret;
debug("%s: rate = %u\n", __func__, priv->input_rate);
priv->bits_per_word = 8;
priv->tmode = TMOD_TR; /* Tx & Rx */
return 0;
}
static int rsb_set_device_mode(void)
{
struct sunxi_rsb_reg * const rsb =
(struct sunxi_rsb_reg *)SUNXI_RSB_BASE;
unsigned long tmo = timer_get_us() + 1000000;
writel(RSB_DMCR_DEVICE_MODE_START | RSB_DMCR_DEVICE_MODE_DATA,
&rsb->dmcr);
while (readl(&rsb->dmcr) & RSB_DMCR_DEVICE_MODE_START) {
if (timer_get_us() > tmo)
return -ETIME;
}
return rsb_await_trans();
}
/**
* Handle the next stage of device init
*/
static int handle_stage(const void *blob, struct tegra_lcd_priv *priv)
{
debug("%s: stage %d\n", __func__, priv->stage);
/* do the things for this stage */
switch (priv->stage) {
case STAGE_START:
/*
* It is possible that the FDT has requested that the LCD be
* disabled. We currently don't support this. It would require
* changes to U-Boot LCD subsystem to have LCD support
* compiled in but not used. An easier option might be to
* still have a frame buffer, but leave the backlight off and
* remove all mention of lcd in the stdout environment
* variable.
*/
funcmux_select(PERIPH_ID_DISP1, FUNCMUX_DEFAULT);
break;
case STAGE_PANEL_VDD:
if (dm_gpio_is_valid(&priv->panel_vdd))
dm_gpio_set_value(&priv->panel_vdd, 1);
break;
case STAGE_LVDS:
if (dm_gpio_is_valid(&priv->lvds_shutdown))
dm_gpio_set_value(&priv->lvds_shutdown, 1);
break;
case STAGE_BACKLIGHT_VDD:
if (dm_gpio_is_valid(&priv->backlight_vdd))
dm_gpio_set_value(&priv->backlight_vdd, 1);
break;
case STAGE_PWM:
/* Enable PWM at 15/16 high, 32768 Hz with divider 1 */
pinmux_set_func(PMUX_PINGRP_GPU, PMUX_FUNC_PWM);
pinmux_tristate_disable(PMUX_PINGRP_GPU);
pwm_set_config(priv->pwm, priv->pwm_channel, 0xdf, 0xff);
pwm_set_enable(priv->pwm, priv->pwm_channel, true);
break;
case STAGE_BACKLIGHT_EN:
if (dm_gpio_is_valid(&priv->backlight_en))
dm_gpio_set_value(&priv->backlight_en, 1);
break;
case STAGE_DONE:
break;
}
/* set up timer for next stage */
priv->timer_next = timer_get_us();
if (priv->stage < FDT_LCD_TIMINGS)
priv->timer_next += priv->panel_timings[priv->stage] * 1000;
/* move to next stage */
priv->stage++;
return 0;
}
//funckje zwraca TRUE jeœli od jej ostatniego wywo³ania up³yne³o
uint8_t timer_new_tick(void){
static uint16_t time_stamp=0;
if(timer_time_elapsed_us(time_stamp) < MAIN_LOOP_PERIOD){
return 0;
}
time_stamp = timer_get_us();
return 1;
};
//zwraca czas w 4us jaki czas up³yn¹³ od wartoœci podanej jako argument (argument czas timera)
//Mierzy czas do 262ms!! potem siê przewija licznik!!
uint16_t timer_time_elapsed_us(uint16_t time_stamp){
uint16_t current_timer=timer_get_us();
if( current_timer >= time_stamp){
return (current_timer-time_stamp);
}
else{
return (current_timer+(0xFFFF-time_stamp));
}
};
static void spi_cs_deactivate(struct udevice *dev)
{
struct udevice *bus = dev->parent;
struct tegra_spi_platdata *pdata = dev_get_platdata(bus);
struct tegra30_spi_priv *priv = dev_get_priv(bus);
/* CS is negated on Tegra, so drive a 0 to get a 1 */
clrbits_le32(&priv->regs->command, SLINK_CMD_CS_VAL);
/* Remember time of this transaction so we can honour the bus delay */
if (pdata->deactivate_delay_us)
priv->last_transaction_us = timer_get_us();
}
static int tegra30_spi_probe(struct udevice *bus)
{
struct tegra_spi_platdata *plat = dev_get_platdata(bus);
struct tegra30_spi_priv *priv = dev_get_priv(bus);
priv->regs = (struct spi_regs *)plat->base;
priv->last_transaction_us = timer_get_us();
priv->freq = plat->frequency;
priv->periph_id = plat->periph_id;
return 0;
}
static void spi_cs_deactivate(struct udevice *dev, uint cs)
{
struct udevice *bus = dev->parent;
struct rockchip_spi_platdata *plat = bus->platdata;
struct rockchip_spi_priv *priv = dev_get_priv(bus);
struct rockchip_spi *regs = priv->regs;
debug("deactivate cs%u\n", cs);
writel(0, ®s->ser);
/* Remember time of this transaction so we can honour the bus delay */
if (plat->deactivate_delay_us)
priv->last_transaction_us = timer_get_us();
}
ulong get_timer_masked(void)
{
ulong now;
/* current tick value */
now = timer_get_us() / (TIMER_CLK / CONFIG_SYS_HZ);
if (now >= gd->lastinc) /* normal mode (non roll) */
/* move stamp forward with absolute diff ticks */
gd->tbl += (now - gd->lastinc);
else /* we have rollover of incrementer */
gd->tbl += ((TIMER_LOAD_VAL / (TIMER_CLK / CONFIG_SYS_HZ))
- gd->lastinc) + now;
gd->lastinc = now;
return gd->tbl;
}
static int tegra20_sflash_probe(struct udevice *bus)
{
struct tegra_spi_platdata *plat = dev_get_platdata(bus);
struct tegra20_sflash_priv *priv = dev_get_priv(bus);
priv->regs = (struct spi_regs *)plat->base;
priv->last_transaction_us = timer_get_us();
priv->freq = plat->frequency;
priv->periph_id = plat->periph_id;
/* Change SPI clock to correct frequency, PLLP_OUT0 source */
clock_start_periph_pll(priv->periph_id, CLOCK_ID_PERIPH,
priv->freq);
return 0;
}
static void spi_cs_activate(struct udevice *dev)
{
struct udevice *bus = dev->parent;
struct tegra_spi_platdata *pdata = dev_get_platdata(bus);
struct tegra30_spi_priv *priv = dev_get_priv(bus);
/* If it's too soon to do another transaction, wait */
if (pdata->deactivate_delay_us &&
priv->last_transaction_us) {
ulong delay_us; /* The delay completed so far */
delay_us = timer_get_us() - priv->last_transaction_us;
if (delay_us < pdata->deactivate_delay_us)
udelay(pdata->deactivate_delay_us - delay_us);
}
/* CS is negated on Tegra, so drive a 1 to get a 0 */
setbits_le32(&priv->regs->command, SLINK_CMD_CS_VAL);
}
/* Called by macro WATCHDOG_RESET */
void watchdog_reset(void)
{
# if !defined(CONFIG_SPL_BUILD)
static ulong next_reset;
ulong now;
if (!watchdog_dev)
return;
now = timer_get_us();
/* Do not reset the watchdog too often */
if (now > next_reset) {
wdt_reset(watchdog_dev);
next_reset = now + 1000;
}
# endif
}
void icmp_received(unsigned char *etherframe,int etherlen)
{
char tmp[80];
ethernet_frame_str *ef = (ethernet_frame_str *)etherframe;
ip_headder_str *ip = (ip_headder_str *)&ef->payload[0];
// イーサフレームのMACアドレスを入れ替える
memcpy(tmp,ef->dst_macaddr,6);
memcpy(ef->dst_macaddr,ef->src_macaddr,6);
memcpy(ef->src_macaddr,tmp,6);
// IPヘッダのIPアドレスを入れ替える
unsigned long tmpaddr = ip->dest_addr;
ip->dest_addr = ip->src_addr;
ip->src_addr = tmpaddr;
ip->checksum = 0;
// IPヘッダの処理
unsigned char *icmpdata = (unsigned char *)ip + (ip->version_length & 0x0f) * 4;
/*
sprintf(tmp,"\n[ICMP received %d bytes]\n",etherlen);
sci_puts(tmp);
packet_dump(etherframe,etherlen);
sprintf(tmp,"ICMP Type %02x , process %d , ping seq %d \n",icmpdata[0], (icmpdata[5] << 8) | icmpdata[4] , (icmpdata[7] << 8) | icmpdata[6]);
sci_puts(tmp);
*/
if(icmpdata[0] == 0x00) // 受信したものがPingの応答だったら
{
printf("Reply from %ld.%ld.%ld.%ld:",ip->dest_addr & 0xff,(ip->dest_addr >> 8) & 0xff,(ip->dest_addr >> 16) & 0xff,(ip->dest_addr >> 24) & 0xff);
printf("bytes=%d ",__builtin_rx_revw(ip->dglength));
printf("time=%ld[us] ttl=%d\n",timer_get_us() - g_ping_timer,ip->ttl);
g_ping_process = 0; // Pingは進行していない
return ;
}
static int tegra186_bpmp_call(struct udevice *dev, int mrq, void *tx_msg,
int tx_size, void *rx_msg, int rx_size)
{
struct tegra186_bpmp *priv = dev_get_priv(dev);
int ret, err;
void *ivc_frame;
struct mrq_request *req;
struct mrq_response *resp;
ulong start_time;
debug("%s(dev=%p, mrq=%u, tx_msg=%p, tx_size=%d, rx_msg=%p, rx_size=%d) (priv=%p)\n",
__func__, dev, mrq, tx_msg, tx_size, rx_msg, rx_size, priv);
if ((tx_size > BPMP_IVC_FRAME_SIZE) || (rx_size > BPMP_IVC_FRAME_SIZE))
return -EINVAL;
ret = tegra_ivc_write_get_next_frame(&priv->ivc, &ivc_frame);
if (ret) {
error("tegra_ivc_write_get_next_frame() failed: %d\n", ret);
return ret;
}
req = ivc_frame;
req->mrq = mrq;
req->flags = BPMP_FLAG_DO_ACK | BPMP_FLAG_RING_DOORBELL;
memcpy(req + 1, tx_msg, tx_size);
ret = tegra_ivc_write_advance(&priv->ivc);
if (ret) {
error("tegra_ivc_write_advance() failed: %d\n", ret);
return ret;
}
start_time = timer_get_us();
for (;;) {
ret = tegra_ivc_channel_notified(&priv->ivc);
if (ret) {
error("tegra_ivc_channel_notified() failed: %d\n", ret);
return ret;
}
ret = tegra_ivc_read_get_next_frame(&priv->ivc, &ivc_frame);
if (!ret)
break;
/* Timeout 20ms; roughly 10x current max observed duration */
if ((timer_get_us() - start_time) > 20 * 1000) {
error("tegra_ivc_read_get_next_frame() timed out (%d)\n",
ret);
return -ETIMEDOUT;
}
}
resp = ivc_frame;
err = resp->err;
if (!err && rx_msg && rx_size)
memcpy(rx_msg, resp + 1, rx_size);
ret = tegra_ivc_read_advance(&priv->ivc);
if (ret) {
error("tegra_ivc_write_advance() failed: %d\n", ret);
return ret;
}
if (err) {
error("BPMP responded with error %d\n", err);
/* err isn't a U-Boot error code, so don't that */
return -EIO;
}
return rx_size;
}
uint64_t VbExGetTimer(void)
{
return timer_get_us();
}
void updateYPR(void) {
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
// If intPin goes high, all data registers have new data
if (MPU9250_I2C_ByteRead(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
ax = (float) accelCount[0] * aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float) accelCount[1] * aRes - accelBias[1];
az = (float) accelCount[2] * aRes - accelBias[2];
readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
gx = (float) gyroCount[0] * gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
gy = (float) gyroCount[1] * gRes - gyroBias[1];
gz = (float) gyroCount[2] * gRes - gyroBias[2];
readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float) magCount[0] * mRes * magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float) magCount[1] * mRes * magCalibration[1] - magbias[1];
mz = (float) magCount[2] * mRes * magCalibration[2] - magbias[2];
Now = timer_get_us();
deltat = (float) ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat;
sumCount++;
//MadgwickQuaternionUpdate(ax, ay, az, gx*_PI/180.0f, gy*_PI/180.0f, gz*_PI/180.0f, my, mx, mz);
MahonyQuaternionUpdate(ax, ay, az, gx * _PI / 180.0f, gy * _PI / 180.0f, gz * _PI / 180.0f, my, mx, mz);
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
ypr[0] = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]),
q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
// q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[1] * q[2] - q[0] * q[3]),
// q[0] * q[0] + q[1] * q[1] - 1);
ypr[1] = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ypr[2] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
ypr[1] *= 180.0f / _PI;
ypr[0] *= 180.0f / _PI;
ypr[0] -= -5.08f; // Declination at Hefei, Anhui is 5 degrees and 5 minutes (negative) on 2015-08-08
ypr[2] *= 180.0f / _PI;
}
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = systemTime - count;
if (delt_t > 1500) { // update LCD once per half-second independent of read rate
trace_printf("ax = %f", 1000 * ax);
trace_printf(" ay = %f", 1000 * ay);
trace_printf(" az = %f mg\n", 1000 * az);
trace_printf("gx = %f", gx);
trace_printf(" gy = %f", gy);
trace_printf(" gz = %f deg/s\n", gz);
trace_printf("gx = %f", mx);
trace_printf(" gy = %f", my);
trace_printf(" gz = %f mG\n", mz);
tempCount = readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
trace_printf("temperature = %f C\n", temperature);
trace_printf("q0 = %f\n", q[0]);
trace_printf("q1 = %f\n", q[1]);
trace_printf("q2 = %f\n", q[2]);
trace_printf("q3 = %f\n", q[3]);
trace_printf("Yaw, Pitch, Roll: %f %f %f\n", ypr[0], ypr[1], ypr[2]);
trace_printf("average rate = %f\n\n\n\r", (float) sumCount / sum);
count = systemTime;
if (count > 1 << 21) {
systemTime = 0; // start the timer over again if ~30 minutes has passed
count = 0;
deltat = 0;
lastUpdate = timer_get_us();
}
sum = 0;
sumCount = 0;
}
}
/**
* Handle the next stage of device init
*/
static int handle_stage(const void *blob)
{
debug("%s: stage %d\n", __func__, stage);
/* do the things for this stage */
switch (stage) {
case STAGE_START:
/* Initialize the Tegra display controller */
if (tegra_display_probe(gd->fdt_blob, (void *)gd->fb_base)) {
printf("%s: Failed to probe display driver\n",
__func__);
return -1;
}
/* get panel details */
if (fdt_decode_lcd(blob, &config)) {
printf("No valid LCD information in device tree\n");
return -1;
}
/*
* It is possible that the FDT has requested that the LCD be
* disabled. We currently don't support this. It would require
* changes to U-Boot LCD subsystem to have LCD support
* compiled in but not used. An easier option might be to
* still have a frame buffer, but leave the backlight off and
* remove all mention of lcd in the stdout environment
* variable.
*/
funcmux_select(PERIPH_ID_DISP1, FUNCMUX_DEFAULT);
fdtdec_setup_gpio(&config.panel_vdd);
fdtdec_setup_gpio(&config.lvds_shutdown);
fdtdec_setup_gpio(&config.backlight_vdd);
fdtdec_setup_gpio(&config.backlight_en);
/*
* TODO: If fdt includes output flag we can omit this code
* since fdtdec_setup_gpio will do it for us.
*/
if (fdt_gpio_isvalid(&config.panel_vdd))
gpio_direction_output(config.panel_vdd.gpio, 0);
if (fdt_gpio_isvalid(&config.lvds_shutdown))
gpio_direction_output(config.lvds_shutdown.gpio, 0);
if (fdt_gpio_isvalid(&config.backlight_vdd))
gpio_direction_output(config.backlight_vdd.gpio, 0);
if (fdt_gpio_isvalid(&config.backlight_en))
gpio_direction_output(config.backlight_en.gpio, 0);
break;
case STAGE_PANEL_VDD:
if (fdt_gpio_isvalid(&config.panel_vdd))
gpio_direction_output(config.panel_vdd.gpio, 1);
break;
case STAGE_LVDS:
if (fdt_gpio_isvalid(&config.lvds_shutdown))
gpio_set_value(config.lvds_shutdown.gpio, 1);
break;
case STAGE_BACKLIGHT_VDD:
if (fdt_gpio_isvalid(&config.backlight_vdd))
gpio_set_value(config.backlight_vdd.gpio, 1);
break;
case STAGE_PWM:
/* Enable PWM at 15/16 high, 32768 Hz with divider 1 */
pinmux_set_func(PINGRP_GPU, PMUX_FUNC_PWM);
pinmux_tristate_disable(PINGRP_GPU);
pwm_enable(config.pwm_channel, 32768, 0xdf, 1);
break;
case STAGE_BACKLIGHT_EN:
if (fdt_gpio_isvalid(&config.backlight_en))
gpio_set_value(config.backlight_en.gpio, 1);
break;
case STAGE_DONE:
break;
}
/* set up timer for next stage */
timer_next = timer_get_us();
if (stage < FDT_LCD_TIMINGS)
timer_next += config.panel_timings[stage] * 1000;
/* move to next stage */
stage++;
return 0;
}
double timer_get_ms(const struct _timer* timer) {
return timer_get_us(timer)/1000.0;
}
