3 BNO055 sensors with a I2C multiplexer comparing values to determine validity

I have three sensors reading pitch and roll, i wrote code to put them in an array, then order the array smallest to largest, then check each value against each other with a tolerance to determine in all three sensors are outputting the same values. My question is am i doing this the most efficient way ?

What this project is for is a amateur high powered rocket, nozzle vectoring guidance system so it always keeps the rocket on the correct pitch and roll angles. I.E. straight up.

//************************
  //LOAD ARRAY
  iRollArray[0] = iRoll0;
  iRollArray[1] = iRoll1;
  iRollArray[2] = iRoll2;
  
  iPitchArray[0] = iPitch0;
  iPitchArray[1] = iPitch1;
  iPitchArray[2] = iPitch2;
  
  iRollSensor[0] = 0;
  iRollSensor[1] = 0;
  iRollSensor[2] = 0;
  
  stSensorName[0] = "S0";
  stSensorName[1] = "S1";
  stSensorName[2] = "S2";
  //************************

  
  // PUT VALUES IN ARRAY IN NUMBERICAL ORDER SMALLEST TO GREATEST
  if(iRollArray[0] > iRollArray[1])
  {
    stTemp = iRollArray[1];
    iRollArray[1] = iRollArray[0];
    iRollArray[0] = iTemp;
  
    stTemp = stSensorName[1];
    stSensorName[1] = stSensorName[0];
    stSensorName[0] = stTemp;
   }
  
  if(iRollArray[1] > iRollArray[2])
  {
    stTemp = iRollArray[2];
    iRollArray[2] = iRollArray[1];
    iRollArray[1] = iTemp;
    
    stTemp = stSensorName[2];
    stSensorName[2] = stSensorName[1];
    stSensorName[1] = stTemp;
  }
  // DONE ORDERING ARRAY

  // CHECK TO SEE IF ALL THREE ARRAY VALUES ARE WITHIN TOLERANCE OF EACH OTHER AND MARK iRollSensor ARRAY WITH A 1 IF THEY ARE
  if(iRollArray[0] >= iRollArray[1] * (1 - fTolerance) && iRollArray[0] <= iRollArray[1] * (1 + fTolerance))
  {
    iRollSensor[0] = 1;
    iRollSensor[1] = 1;
  }

  if(iRollArray[1] >= iRollArray[2] * (1 - fTolerance) && iRollArray[1] <= iRollArray[2] * (1 + fTolerance))
  {
    iRollSensor[1] = 1;
    iRollSensor[2] = 1;
  }
  // DONE CHECKING VALUES AGAINST EACH OTHER
  
  // MULTIPLY EACH VALUE TIMES iRollSensor ARRAY EITHER 1 OR 0, IT IS IS A 0 THEN IT IS CONSIDERED INVALID AND WILL NOT BE USED IN THE ANGLE CACULATION
  iRollValid = ((iRollArray[0] * iRollSensor[0]) + (iRollArray[1] * iRollSensor[1]) + (iRollArray[2] * iRollSensor[2])) / (iRollSensor[0] + iRollSensor[1] + iRollSensor[2]);

  //RINSE AND REPEAT FOR PITCH VALUE
  iPitchSensor[0] = 0;
  iPitchSensor[1] = 0;
  iPitchSensor[2] = 0;
  
  stSensorName[0] = "S0";
  stSensorName[1] = "S1";
  stSensorName[2] = "S2";
  
  // PUT VALUES IN ARRAY IN NUMERICAL ORDER SMALLEST TO GREATEST
  if(iPitchArray[0] > iPitchArray[1])
  {
    stTemp = iPitchArray[1];
    iPitchArray[1] = iPitchArray[0];
    iPitchArray[0] = iTemp;
  
    stTemp = stSensorName[1];
    stSensorName[1] = stSensorName[0];
    stSensorName[0] = stTemp;
   }
  
  if(iPitchArray[1] > iPitchArray[2])
  {
    stTemp = iPitchArray[2];
    iPitchArray[2] = iPitchArray[1];
    iPitchArray[1] = iTemp;
    
    stTemp = stSensorName[2];
    stSensorName[2] = stSensorName[1];
    stSensorName[1] = stTemp;
  }
  // DONE ORDERING ARRAY

  // CHECK TO SEE IF ALL THREE ARRAY VALUES ARE WITHIN TOLERANCE OF EACH OTHER AND MARK ISENSOR ARRAY WITH A 1 IF THEY ARE
  if(iPitchArray[0] >= iPitchArray[1] * (1 - fTolerance) && iPitchArray[0] <= iPitchArray[1] * (1 + fTolerance))
  {
    iPitchSensor[0] = 1;
    iPitchSensor[1] = 1;
  }

  if(iPitchArray[1] >= iPitchArray[2] * (1 - fTolerance) && iPitchArray[1] <= iPitchArray[2] * (1 + fTolerance))
  {
    iPitchSensor[1] = 1;
    iPitchSensor[2] = 1;
  }
  // DONE CHECKING VALUES AGAINST EACH OTHER

  // MULTIPLY EACH VALUE TIMES ISENSOR ARRAY EITHER 1 OR 0, IT IS IS A 0 THEN IT IS CONSIDERED INVALID AND WILL NOT BE USED IN THE ANGLE CACULATION
  iPitchValid = ((iPitchArray[0] * iPitchSensor[0]) + (iPitchArray[1] * iPitchSensor[1]) + (iPitchArray[2] * iPitchSensor[2])) / (iPitchSensor[0] + iPitchSensor[1] + iPitchSensor[2]);