What is the bitshifting doing here

calsa · August 25, 2023, 7:40pm

I downloaded source for an ESP32 based drone flight controller, but don't quite understand the context of the bit shifting that is done throughout. I only ever use bitshifting on low-level hardware that can't support floating point and for things like serial packet deconstruction.

Can someone explain why that approach is used, and how its actually working?

Also, what is the deal with four reassignments of the same variable (GPOS, GPOC both do this) in a row with different shift values?

#include <Wire.h>
#include <WiFi.h>
#include <WiFiUdp.h>
#include <WebServer.h>

WebServer server(80);
char* SSID = "NetworkName";
char* SSIDPW = "NOPE"

WiFiUDP UDP;


char packet[7];
boolean recvState;
//-----------------------------------------------------------------------//
int ESCout_1, ESCout_2, ESCout_3, ESCout_4;
int input_PITCH = 50;
int input_ROLL = 50;
int input_YAW;
int input_THROTTLE = 1100;
int state1, state2, state3, state4;
//-----------------------------------------------------------------------//
float GPOS;
float GPOC;
int16_t gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temperature, acc_total_vector;
float angle_pitch, angle_roll, angle_yaw;
boolean set_gyro_angles;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch_output, angle_roll_output;
float elapsedTime;
long Time, timePrev, time2;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
//-----------------------------------------------------------------------//
float pitch_PID, roll_PID, yaw_PID;
float roll_error, roll_previous_error, pitch_error, pitch_previous_error, yaw_error;
float roll_pid_p, roll_pid_d, roll_pid_i, pitch_pid_p, pitch_pid_i, pitch_pid_d, yaw_pid_p, yaw_pid_i;
float roll_desired_angle, pitch_desired_angle, yaw_desired_angle;
double twoX_kp = 5;
double twoX_ki = 0.003;
double twoX_kd = 2;
double yaw_kp = 3;
double yaw_ki = 0.002;
//-----------------------------------------------------------------------//
void setup() {
  pinMode(D5, OUTPUT);  // Set Motor Pin 5 to Output Mode
  pinMode(D6, OUTPUT);  // Set Motor Pin 6 to Output Mode
  pinMode(D7, OUTPUT);  // Set Motor Pin 7 to Output Mode
  pinMode(D8, OUTPUT);  // Set Motor Pin 8 to Output Mode

  delay(1300);
  Serial.begin(115200);
  Serial.println("Connecting to ");
  Serial.println(ssid);
  WiFi.mode(WIFI_STA);  // Connect to an AP
  WiFi.begin(SSID, SSIDPW);
  while (WiFi.status() != WL_CONNECTED) {
    GPOS = (1 << 14);
    GPOS = (1 << 12);
    GPOS = (1 << 13);
    GPOS = (1 << 15);
    delayMicroseconds(1000);
    GPOC = (1 << 14);
    GPOC = (1 << 12);
    GPOC = (1 << 13);
    GPOC = (1 << 15);
    delayMicroseconds(1000);
    yield();
  }

  Serial.println("");
  Serial.println("WiFi connected..!");
  Serial.print("Got IP: ");
  Serial.println(WiFi.localIP());


  UDP.begin(9999);


  //-----------------------------------------------------------------------//
  Wire.begin();
  Wire.setClock(400000);
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
  delay(1000);
  for (int cal_int = 0; cal_int < 2000; cal_int++) {
    if (cal_int % 125 == 0) Serial.print(".");
    Wire.beginTransmission(0x68);
    Wire.write(0x3B);
    Wire.endTransmission();
    Wire.requestFrom(0x68, 14);
    while (Wire.available() < 14)
      ;
    acc_x = Wire.read() << 8 | Wire.read();
    acc_y = Wire.read() << 8 | Wire.read();
    acc_z = Wire.read() << 8 | Wire.read();
    temperature = Wire.read() << 8 | Wire.read();
    gyro_x = Wire.read() << 8 | Wire.read();
    gyro_y = Wire.read() << 8 | Wire.read();
    gyro_z = Wire.read() << 8 | Wire.read();
    gyro_x_cal += gyro_x;
    gyro_y_cal += gyro_y;
    gyro_z_cal += gyro_z;
    GPOS = (1 << 14);
    GPOS = (1 << 12);
    GPOS = (1 << 13);
    GPOS = (1 << 15);
    delayMicroseconds(1000);
    GPOC = (1 << 14);
    GPOC = (1 << 12);
    GPOC = (1 << 13);
    GPOC = (1 << 15);
    delayMicroseconds(1000);
    yield();
  }
  gyro_x_cal /= 2000;
  gyro_y_cal /= 2000;
  gyro_z_cal /= 2000;


  //-----------------------------------------------------------------------//
  Time = micros();
}

void loop() {
  GPOS = (1 << 14);
  GPOS = (1 << 12);
  GPOS = (1 << 13);
  GPOS = (1 << 15);
  timePrev = Time;
  Time = micros();
  elapsedTime = (float)(Time - timePrev) / (float)1000000;
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);
  while (Wire.available() < 14);
  acc_x = Wire.read() << 8 | Wire.read();
  acc_y = Wire.read() << 8 | Wire.read();
  acc_z = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() << 8 | Wire.read();
  gyro_x = Wire.read() << 8 | Wire.read();
  gyro_y = Wire.read() << 8 | Wire.read();
  gyro_z = Wire.read() << 8 | Wire.read();
  gyro_x -= gyro_x_cal;
  gyro_y -= gyro_y_cal;
  gyro_z -= gyro_z_cal;
  angle_pitch += gyro_x * elapsedTime * 0.01526717557;
  angle_roll += gyro_y * elapsedTime * 0.01526717557;
  angle_yaw += gyro_z * elapsedTime * 0.01526717557;
  angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
  angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
  acc_total_vector = sqrt((acc_x * acc_x) + (acc_y * acc_y) + (acc_z * acc_z));
  angle_pitch_acc = asin((float)acc_y / acc_total_vector) * 57.296;
  angle_roll_acc = asin((float)acc_x / acc_total_vector) * -57.296;
  angle_pitch_acc += 0;
  angle_roll_acc += 0;
  if (set_gyro_angles) {
    angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
    angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
  } else {
    angle_pitch = angle_pitch_acc;
    angle_roll = angle_roll_acc;
    set_gyro_angles = true;
  }
  angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
  angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;



  //-----------------------------------------------------------------------//
  roll_desired_angle = 3 * ((float)input_ROLL / (float)10 - (float)5);
  pitch_desired_angle = 3 * ((float)input_PITCH / (float)10 - (float)5);
  //yaw_desired_angle =0;

  roll_error = angle_roll_output - roll_desired_angle;
  pitch_error = angle_pitch_output - pitch_desired_angle;
  yaw_error = angle_yaw - yaw_desired_angle;

  roll_pid_p = twoX_kp * roll_error;
  pitch_pid_p = twoX_kp * pitch_error;
  yaw_pid_p = yaw_kp * yaw_error;

  if (-3 < roll_error < 3) { roll_pid_i = roll_pid_i + (twoX_ki * roll_error); }
  if (-3 < pitch_error < 3) { pitch_pid_i = pitch_pid_i + (twoX_ki * pitch_error); }
  if (-3 < yaw_error < 3) { yaw_pid_i = yaw_pid_i + (yaw_ki * yaw_error); }

  roll_pid_d = twoX_kd * ((roll_error - roll_previous_error) / elapsedTime);
  pitch_pid_d = twoX_kd * ((pitch_error - pitch_previous_error) / elapsedTime);
  roll_PID = roll_pid_p + roll_pid_i + roll_pid_d;
  pitch_PID = pitch_pid_p + pitch_pid_i + pitch_pid_d;
  yaw_PID = yaw_pid_p + yaw_pid_i;

  if (roll_PID < -400) {
    roll_PID = -400;
  } else if (roll_PID > 400) {
    roll_PID = 400;
  }
  if (pitch_PID < -400) {
    pitch_PID = -400;
  } else if (pitch_PID > 400) {
    pitch_PID = 400;
  }
  if (yaw_PID < -400) {
    yaw_PID = -400;
  } else if (yaw_PID > 400) {
    yaw_PID = 400;
  }

  ESCout_1 = input_THROTTLE - roll_PID - pitch_PID - yaw_PID;
  ESCout_2 = input_THROTTLE + roll_PID - pitch_PID + yaw_PID;
  ESCout_3 = input_THROTTLE + roll_PID + pitch_PID - yaw_PID;
  ESCout_4 = input_THROTTLE - roll_PID + pitch_PID + yaw_PID;

  if (ESCout_1 > 2000) ESCout_1 = 2000;
  else if (ESCout_1 < 1100) ESCout_1 = 1100;
  if (ESCout_2 > 2000) ESCout_2 = 2000;
  else if (ESCout_2 < 1100) ESCout_2 = 1100;
  if (ESCout_3 > 2000) ESCout_3 = 2000;
  else if (ESCout_3 < 1100) ESCout_3 = 1100;
  if (ESCout_4 > 2000) ESCout_4 = 2000;
  else if (ESCout_4 < 1100) ESCout_4 = 1100;

  roll_previous_error = roll_error;
  pitch_previous_error = pitch_error;
  //-----------------------------------------------------------------------//
  while ((micros() - Time) < 1000);
  state1 = 1;
  state2 = 1;
  state3 = 1;
  state4 = 1;
  while (state1 == 1 || state2 == 1 || state3 == 1 || state4 == 1) {
    time2 = micros();
    if ((time2 - Time) >= ESCout_1 && state1 == 1) {
      GPOC = (1 << 14);
      state1 = 0;
    }
    if ((time2 - Time) >= ESCout_2 && state2 == 1) {
      GPOC = (1 << 12);
      state2 = 0;
    }
    if ((time2 - Time) >= ESCout_3 && state3 == 1) {
      GPOC = (1 << 13);
      state3 = 0;
    }
    if ((time2 - Time) >= ESCout_4 && state4 == 1) {
      GPOC = (1 << 15);
      state4 = 0;
    }
  }
  //-----------------------------------------------------------------------//
  if (!recvState) {
    int packetSize = UDP.parsePacket();
    if (packetSize) {
      int len = UDP.read(packet, 6);
      packet[len] = '\0';
      if (String(packet[0]) == "a") {
        input_ROLL = int(packet[1]);
        input_PITCH = int(packet[2]);
        input_THROTTLE = 1000 + int(packet[3]);
        input_YAW = int(packet[4]);
      } else if (String(packet[0]) == "b") {
        input_ROLL = int(packet[1]);
        input_PITCH = int(packet[2]);
        input_THROTTLE = 1000 + int(packet[3]) * 100 + int(packet[4]);
        input_YAW = int(packet[5]);
      }
      if (String(packet[0]) == "1") {
        twoX_kp = (float)int(packet[1]) / (float)100;
        twoX_ki = (float)int(packet[2]) / (float)1000;
        twoX_kd = (float)int(packet[3]) / (float)100;
      } else if (String(packet[0]) == "2") {
        twoX_kp = (float)(float)(int(packet[1]) * 100 + int(packet[2])) / (float)100;
        twoX_ki = (float)(float)(int(packet[3]) * 100 + int(packet[4])) / (float)1000;
        twoX_kd = (float)(float)(int(packet[5]) * 100 + int(packet[6])) / (float)100;
      } else if (String(packet[0]) == "3") {
        twoX_kp = (float)(int(packet[1]) * 100 + int(packet[2])) / (float)100;
        twoX_ki = (float)int(packet[3]) / (float)1000;
        twoX_kd = (float)int(packet[4]) / (float)100;
      } else if (String(packet[0]) == "4") {
        twoX_kp = (float)int(packet[1]) / (float)100;
        twoX_ki = (float)(int(packet[2]) * 100 + int(packet[3])) / (float)1000;
        twoX_kd = (float)int(packet[4]) / (float)100;
      } else if (String(packet[0]) == "5") {
        twoX_kp = (float)int(packet[1]) / (float)100;
        twoX_ki = (float)int(packet[2]) / (float)1000;
        twoX_kd = (float)(int(packet[3]) * 100 + int(packet[4])) / (float)100;
      } else if (String(packet[0]) == "6") {
        twoX_kp = (float)(int(packet[1]) * 100 + int(packet[2])) / (float)100;
        twoX_ki = (float)(int(packet[3]) * 100 + int(packet[4])) / (float)1000;
        twoX_kd = (float)int(packet[5]) / (float)100;
      } else if (String(packet[0]) == "7") {
        twoX_kp = (float)int(packet[1]) / (float)100;
        twoX_ki = (float)(int(packet[2]) * 100 + int(packet[3])) / (float)1000;
        twoX_kd = (float)(int(packet[4]) * 100 + int(packet[5])) / (float)100;
      } else if (String(packet[0]) == "8") {
        twoX_kp = (float)(int(packet[1]) * 100 + int(packet[2])) / (float)100;
        twoX_ki = (float)int(packet[3]) / (float)1000;
        twoX_kd = (float)(int(packet[4]) * 100 + int(packet[5])) / (float)100;
      }
      Serial.print(input_ROLL);
      Serial.print(" ");\
      Serial.print(input_THROTTLE);
      Serial.print(" ");
      Serial.print(twoX_kp);
      Serial.print(" ");
      Serial.print(twoX_ki, 3);
      Serial.print(" ");
      Serial.print(twoX_kd);
      Serial.println();
    }
  } else if (recvState) {
    UDP.beginPacket(UDP.remoteIP(), UDP.remotePort());
    UDP.print(Time - timePrev);
    UDP.endPacket();
  }
  recvState = !recvState;
  //-----------------------------------------------------------------------//
  Serial.print(angle_roll_output);
  Serial.print("  ");
  Serial.print(angle_pitch_output);
  Serial.print(" | ");
  Serial.print(roll_desired_angle);
  Serial.print("  ");
  Serial.print(pitch_desired_angle);
  Serial.println();
}

jremington · August 25, 2023, 7:43pm

Makes no sense to me. Ask the code developer.

If you actually intend to run the code, take out those bits and see what breaks.

MaximoEsfuerzo · August 25, 2023, 7:46pm

This?

acc_x = Wire.read() << 8 | Wire.read();

Is equal to

acc_x = Wire.read() * 256 + Wire.read();

returns an int reading 2 bytes.

Regards

calsa · August 25, 2023, 7:47pm

Yeah, its old code from a 4+ year old Youtube video. But I wanted some very basic source examples stripped of any non-essential functionality to see others approaches. The goal is to eventually write my own flight control stack from scratch. I figured looking at how others were doing it in as "pure" of control loop as possible would be a good start, but this example with the bit shifting left me wondering "why" as its quite a bit different than how I would write things or other examples I have come across.

jremington · August 25, 2023, 7:48pm

That uncommented code, with nonsensical constructs, is not a good starting point for anything. It is intended to stabilize nearly level flight, and will fail in other situations.

calsa · August 25, 2023, 7:50pm

Its more of things like this, where you shift an assignment, just to seemingly discard the shift and instantly reassign it again. Its nonsensical to me.

The wire.reads actually make the most sense as you would be pulling two bytes from the gyros sensor

DaveX · August 25, 2023, 7:50pm

Maybe it was some cruft once related to some direct IO for pin setting and clearing like:

github.com

arduino/esp8266/blob/1eb0645dcb82e03e239e7932ad20802c769c89e0/cores/esp8266/esp8266_peri.h#L45-L46


      
          #define GPOS   ESP8266_REG(0x304) //GPIO_OUT_SET WO
          #define GPOC   ESP8266_REG(0x308) //GPIO_OUT_CLR WO

jremington · August 25, 2023, 7:51pm

I'm guessing the code was originally intended to blink a row of LEDs, or similar activity.

calsa · August 25, 2023, 7:53pm

Ah, that could make sense. I did not think if it writing to a register as did not see where the ESP would be constantly monitoring it.

jremington · August 25, 2023, 7:56pm

GPOS and GPOC are now declared as floats and are not used for anything, so an optimizing compiler might actually remove all of those statements.

DaveX · August 25, 2023, 7:58pm

It might be leftover code from porting from a processor where those were defined as bit-setting registers. With these declarations might be a workaround to keep the compilation of the poorly-ported code from erroring:

calsa · August 25, 2023, 8:00pm

Thanks everyone, I appreciate all the quick reply. I scratched my head feeling dumb and asking why for a good day on that one.

system · February 21, 2024, 8:01pm

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.

Topic		Replies	Views
What is bitshifting? Programming Questions	5	499	October 9, 2023
Bitwise AND and Bitshifting Programming Questions	6	2783	May 6, 2021
Bitwise shift behaviour Programming Questions	5	1563	May 5, 2021
Bitshift help. Programming Questions	17	5078	May 5, 2021
Bitshifting - Splitting and Combining Programming Questions	6	1083	May 5, 2021

What is the bitshifting doing here

Related Topics