예제 #1
0
// Get rows from other half over i2c
bool transport_master(matrix_row_t matrix[]) {
  i2c_readReg(SLAVE_I2C_ADDRESS, I2C_KEYMAP_START, (void *)matrix, sizeof(i2c_buffer->smatrix), TIMEOUT);

  // write backlight info
#  ifdef BACKLIGHT_ENABLE
  uint8_t level = get_backlight_level();
  if (level != i2c_buffer->backlight_level) {
    if (i2c_writeReg(SLAVE_I2C_ADDRESS, I2C_BACKLIGHT_START, (void *)&level, sizeof(level), TIMEOUT) >= 0) {
      i2c_buffer->backlight_level = level;
    }
  }
#  endif

#  if defined(RGBLIGHT_ENABLE) && defined(RGBLIGHT_SPLIT)
  if (rgblight_get_change_flags()) {
    rgblight_syncinfo_t rgblight_sync;
    rgblight_get_syncinfo(&rgblight_sync);
    if (i2c_writeReg(SLAVE_I2C_ADDRESS, I2C_RGB_START,
                     (void *)&rgblight_sync, sizeof(rgblight_sync), TIMEOUT) >= 0) {
      rgblight_clear_change_flags();
    }
  }
#  endif

#  ifdef ENCODER_ENABLE
  i2c_readReg(SLAVE_I2C_ADDRESS, I2C_ENCODER_START, (void *)i2c_buffer->encoder_state, sizeof(I2C_slave_buffer_t.encoder_state), TIMEOUT);
  encoder_update_raw(i2c_buffer->encoder_state);
#  endif

  return true;
}
예제 #2
0
uint8_t lsm303_read8(uint8_t address, uint8_t reg)
{
  byte value = 0;
  byte buf[1];

  value = i2c_readReg(address,reg,buf,1);

  return value;
}
예제 #3
0
/***************************************************************************
 PUBLIC FUNCTIONS
***************************************************************************/
uint8_t lsm303_read()
{
  byte rx_data[6];

  // Read the accelerometer
  i2c_readReg(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80, rx_data, (byte)6);  

  uint8_t xlo = rx_data[0];
  uint8_t xhi = rx_data[1];
  uint8_t ylo = rx_data[2];
  uint8_t yhi = rx_data[3];
  uint8_t zlo = rx_data[4];
  uint8_t zhi = rx_data[5];

  // Shift values to create properly formed integer (low byte first)
  lsm303accelData.x = (xlo | (xhi << 8)) >> 4;
  lsm303accelData.y = (ylo | (yhi << 8)) >> 4;
  lsm303accelData.z = (zlo | (zhi << 8)) >> 4;

  // Read the magnetometer
  i2c_readReg(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M, rx_data, (byte)6);  

  // Note high before low (different than accel)
  xhi = rx_data[0];
  xlo = rx_data[1];
  zhi = rx_data[2];
  zlo = rx_data[3];
  yhi = rx_data[4];
  ylo = rx_data[5];

  // Shift values to create properly formed integer (low byte first)
  lsm303magData.x = (xlo | (xhi << 8));
  lsm303magData.y = (ylo | (yhi << 8));
  lsm303magData.z = (zlo | (zhi << 8));

  // ToDo: Calculate orientation
  lsm303magData.orientation = 0.0;

  return 0;
}
Bool getTVP5146Status(Ptr statusGS) {
  Bool  resultGS      = FALSE;
  Uint8 bufferGS[2]     = {0};
  PSP_VPFE_TVP5146_StatusParams * statusParamGS;
  
  if (NULL != statusGS) {
    statusParamGS = (PSP_VPFE_TVP5146_StatusParams *)statusGS;
    bufferGS[0] = STATUS_1_REG;
    if(TRUE == i2c_readReg(I2C_TVP_SLAVE_ADDR,bufferGS,2u)) {
      statusParamGS->fieldRate = (Uint8)((bufferGS[1] & 0x20u) ? 50u: 60u);
      statusParamGS->hLock = (Uint8)((bufferGS[1] & 0x02u) >> 1);
      statusParamGS->vLock = (Uint8)((bufferGS[1] & 0x04u) >> 2);
      statusParamGS->lostLock = (Uint8)((bufferGS[1] & 0x10u) >> 4);
      statusParamGS->colSubCarrier_lock = (Uint8)((bufferGS[1] & 0x08u) >> 3);
      resultGS = TRUE;
    }
// To set the TVP6146 mode
Bool setTVP5146Mode(Uint32 modeSTM ) {
  Bool resultSTM      = FALSE;
  Uint8 videoStandardSTM  = 0;
  Uint8 bufferSTM[2]    = {0};

  PAL_osWaitMsecs(100u);

  if ( TRUE == i2c_readReg( I2C_TVP_SLAVE_ADDR, bufferSTM, 2u) ) {
    bufferSTM[0] = OUTPUT_FORMATTER_1_REG;
    bufferSTM[1] |= ((Uint8)((modeSTM & 0x8) << 4)); // 4th-bit for squre pixel sampling 
    if(TRUE == i2c_writeReg( I2C_TVP_SLAVE_ADDR, bufferSTM, 2u)) {
      resultSTM = TRUE;
    }
  }

  if ( resultSTM == TRUE ) {
    resultSTM = FALSE;
    bufferSTM[0] = VIDEO_STANDARD_REG;
    bufferSTM[1] = (Uint8)( modeSTM & 0x7);
    if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR, bufferSTM, 2u)) {
      resultSTM = TRUE;
    }
  }

  if ( resultSTM == TRUE )  {
    resultSTM = FALSE;
    if ( PSP_VPFE_TVP5146_MODE_AUTO == videoStandardSTM ) {
      bufferSTM[0] = AUTOSWITCH_MASK_REG;
      bufferSTM[1] = 0x3F; // enable autoswitch for all standards 
      if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTM,2u)) {
        resultSTM = TRUE;
      }
    } else {
      bufferSTM[0] = AUTOSWITCH_MASK_REG;
      bufferSTM[1] = 0x23; // For NTSC and PAL 
      if(TRUE == i2c_writeReg(I2C_TVP_SLAVE_ADDR,bufferSTM,2u)) {
        resultSTM = TRUE;
      }
    }
  }

  return(resultSTM);
}
예제 #6
0
uint8_t GPS_NewData(void) {
  uint8_t axis;
  #if defined(I2C_GPS)
    static uint8_t _i2c_gps_status;

    //Do not use i2c_writereg, since writing a register does not work if an i2c_stop command is issued at the end
    //Still investigating, however with separated i2c_repstart and i2c_write commands works... and did not caused i2c errors on a long term test.

    GPS_numSat = (_i2c_gps_status & 0xf0) >> 4;
    _i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00);                 //Get status register 

    uint8_t *varptr;
    #if defined(I2C_GPS_SONAR)
      i2c_rep_start(I2C_GPS_ADDRESS<<1);
      i2c_write(I2C_GPS_SONAR_ALT);         
      i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

      varptr = (uint8_t *)&sonarAlt;          // altitude (in cm? maybe)
      *varptr++ = i2c_readAck();
      *varptr   = i2c_readNak();
    #endif	
    
    if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) {                                     //Check is we have a good 3d fix (numsats>5)
      f.GPS_FIX = 1;

      if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) {                                //Check about new data
        GPS_Frame = 1;

        i2c_rep_start(I2C_GPS_ADDRESS<<1);
        i2c_write(I2C_GPS_LOCATION);                //Start read from here 2x2 bytes distance and direction
        i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

        varptr = (uint8_t *)&GPS_coord[LAT];        // for latitude displaying
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();

        varptr = (uint8_t *)&GPS_coord[LON];        // for longitude displaying
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readNak();

        i2c_rep_start(I2C_GPS_ADDRESS<<1);
        i2c_write(I2C_GPS_GROUND_SPEED);          
        i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

        varptr = (uint8_t *)&GPS_speed;          // speed in cm/s for OSD
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();

        varptr = (uint8_t *)&GPS_altitude;       // altitude in meters for OSD
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();	

        varptr = (uint8_t *)&GPS_ground_course;
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readNak();

      } else {
        return 0; 
      }
    } else {
      f.GPS_FIX = 0;
      return 0;
    }
  #endif

  #if defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
    #if defined(GPS_SERIAL)
      uint8_t c = SerialAvailable(GPS_SERIAL);
      if (c==0) return 0;
      while (c--) {
        if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
    #elif defined(GPS_FROM_OSD)
      {
        if(GPS_update & 2) {  // Once second bit of GPS_update is set, indicate new GPS datas is readed from OSD - all in right format.
          GPS_update &= 1;    // We have: GPS_fix(0-2), GPS_numSat(0-15), GPS_coord[LAT & LON](signed, in 1/10 000 000 degres), GPS_altitude(signed, in meters) and GPS_speed(in cm/s)                     
    #endif
          GPS_Frame = 1;
        }
      }
  #endif
  return 1;
}

uint8_t GPS_Compute(void) {
  if (GPS_Frame == 0) return 0; else GPS_Frame = 0;

  if (GPS_update == 1) GPS_update = 0; else GPS_update = 1;
  if (f.GPS_FIX && GPS_numSat >= 5) {
    #if !defined(DONT_RESET_HOME_AT_ARM)
       if (!f.ARMED) {f.GPS_FIX_HOME = 0;}
    #endif
    if (!f.GPS_FIX_HOME && f.ARMED) {
      GPS_reset_home_position();
    }

    //Apply moving average filter to GPS data
    #if defined(GPS_FILTERING)
      GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
      for (uint8_t axis = 0; axis< 2; axis++) {
        GPS_read[axis] = GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
        GPS_degree[axis] = GPS_read[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t
    
        // How close we are to a degree line ? its the first three digits from the fractions of degree
        // later we use it to Check if we are close to a degree line, if yes, disable averaging,
        fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000;
    
        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
        GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000); 
        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
        GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
        if ( nav_mode == NAV_MODE_POSHOLD) {      //we use gps averaging only in poshold mode...
          if ( fraction3[axis]>1 && fraction3[axis]<999 ) GPS_coord[axis] = GPS_filtered[axis];
        }
      }
    #endif

    //dTnav calculation
    //Time for calculating x,y speed and navigation pids
    static uint32_t nav_loopTimer;
    dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
    nav_loopTimer = millis();
    // prevent runup from bad GPS
    dTnav = min(dTnav, 1.0);  

    //calculate distance and bearings for gui and other stuff continously - From home to copter
    uint32_t dist;
    int32_t  dir;
    GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist,&dir);
    GPS_distanceToHome = dist/100;
    GPS_directionToHome = dir/100;

    if (!f.GPS_FIX_HOME) {     //If we don't have home set, do not display anything
      GPS_distanceToHome = 0;
      GPS_directionToHome = 0;
    }

    //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
    GPS_calc_velocity();        

    if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE){    //ok we are navigating 
      //do gps nav calculations here, these are common for nav and poshold  
      #if defined(GPS_LEAD_FILTER)
        GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
        GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
      #else
        GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
        GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
      #endif
      switch (nav_mode) {
        case NAV_MODE_POSHOLD: 
          //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
          GPS_calc_poshold();
          break;
        case NAV_MODE_WP:
          int16_t speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV);      //slow navigation 
          // use error as the desired rate towards the target
          //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
          GPS_calc_nav_rate(speed);

          //Tail control
          if (NAV_CONTROLS_HEADING) {
            if (NAV_TAIL_FIRST) {
              magHold = wrap_18000(nav_bearing-18000)/100;
            } else {
              magHold = nav_bearing/100;
            }
          }
          // Are we there yet ?(within 2 meters of the destination)
          if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){         //if yes switch to poshold mode
            nav_mode = NAV_MODE_POSHOLD;
            if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
          } 
          break;               
      }
    } //end of gps calcs  
  }
}


void GPS_reset_home_position(void) {
  if (f.GPS_FIX && GPS_numSat >= 5) {
    GPS_home[LAT] = GPS_coord[LAT];
    GPS_home[LON] = GPS_coord[LON];
    GPS_calc_longitude_scaling(GPS_coord[LAT]);  //need an initial value for distance and bearing calc
    nav_takeoff_bearing = att.heading;             //save takeoff heading
    //Set ground altitude
    f.GPS_FIX_HOME = 1;
  }
}

//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void) {
  uint8_t i;
  
  for(i=0;i<2;i++) {
    nav_rated[i] = 0;
    nav[i] = 0;
    reset_PID(&posholdPID[i]);
    reset_PID(&poshold_ratePID[i]);
    reset_PID(&navPID[i]);
    nav_mode = NAV_MODE_NONE;
  }
}

//Get the relevant P I D values and set the PID controllers 
void GPS_set_pids(void) {
  posholdPID_PARAM.kP   = (float)conf.pid[PIDPOS].P8/100.0;
  posholdPID_PARAM.kI   = (float)conf.pid[PIDPOS].I8/100.0;
  posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
  
  poshold_ratePID_PARAM.kP   = (float)conf.pid[PIDPOSR].P8/10.0;
  poshold_ratePID_PARAM.kI   = (float)conf.pid[PIDPOSR].I8/100.0;
  poshold_ratePID_PARAM.kD   = (float)conf.pid[PIDPOSR].D8/1000.0;
  poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
  
  navPID_PARAM.kP   = (float)conf.pid[PIDNAVR].P8/10.0;
  navPID_PARAM.kI   = (float)conf.pid[PIDNAVR].I8/100.0;
  navPID_PARAM.kD   = (float)conf.pid[PIDNAVR].D8/1000.0;
  navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
예제 #7
0
파일: accel.c 프로젝트: bern422/gun-loc
bool test_accel(void) {
  uint8_t c;
  return (i2c_readReg(ACCEL_ADDR, WHO_AM_I, &c, 1) == 0) && (c == 0x68);
}
예제 #8
0
파일: accel.c 프로젝트: bern422/gun-loc
void accel_clear_int(void) {
  uint8_t c;
  i2c_readReg(ACCEL_ADDR, INT_ENABLE, &c, 1);
}