   state.heave_fwd ^= Wire.read();
 
   Wire.beginTransmission(ESC_BOARD[4]);
-  Wire.write(valid(cmd.pwm.heave_port_aft >> 8));
+  Wire.write(valid(cmd.pwm.heave_port_aft) >> 8);
   Wire.write(valid(cmd.pwm.heave_port_aft));
-  Wire.write(valid(cmd.pwm.heave_stbd_aft >> 8));
+  Wire.write(valid(cmd.pwm.heave_stbd_aft) >> 8);
   Wire.write(valid(cmd.pwm.heave_stbd_aft));
   Wire.endTransmission();
 

  //

  Wire.beginTransmission(ESC_BOARD[0]);
  Wire.write(valid(cmd.pwm.surge_port_hi >> 8));
  Wire.write(valid(cmd.pwm.surge_port_hi));
  Wire.write(valid(cmd.pwm.surge_stbd_hi >> 8));
  Wire.write(valid(cmd.pwm.surge_stbd_hi));
  Wire.endTransmission();


  state.sway ^= Wire.read();

  Wire.beginTransmission(ESC_BOARD[3]);
  Wire.write(valid(cmd.pwm.heave_port_fwd) >> 8);
  Wire.write(valid(cmd.pwm.heave_port_fwd));
  Wire.write(valid(cmd.pwm.heave_stbd_fwd) >> 8);
  Wire.write(valid(cmd.pwm.heave_stbd_fwd));
  Wire.endTransmission();

