Mavlink Protocol from arduino to Pixhawk Mission Planner

I am looking advice in amending this code to write the data out into a mavlink packet rather than to the led screen, to be monitored in Mission planner through the pixhawk mavlink telemetry. Any advice would be appreciated, I am pretty green to raw coding.

Programmed for the Teensy-LC
// Uses a 128 X 32 OLED, SSD1306 controller
// Arduino IDE 1.8.5 and Teensyduino 1.41
// Uses the U8g2 Library for monochrome displays, version 2.20.13

#include <U8g2lib.h>
#include <Wire.h>

// function prototypes
void conPM(int PMvalue, char *charArray);
bool StopAuto(void);
bool ReadPM(void);

#define HWSERIAL Serial1 // Rx on 0 and Tx on 1 for LC

// oled definition
U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0);

// prefixes for each line
//const char s25 = {’ ‘, ‘2’, ‘.’, ‘5’, 181, ‘=’, 0};
//const char s100 = {‘1’, ‘0’, ‘.’, ‘0’, 181, ‘=’, 0};
const char s25 = {’ ', ‘2’, ‘.’, ‘5’, 181, ‘m’, ‘=’, 0};
const char s100 = {‘1’, ‘0’, ‘.’, ‘0’, 181, ‘m’, ‘=’, 0};
// storage of particle values as char array (after conversion)
// we will display at [1] since max value is 1000 as per datasheet
char v25 = {‘6’, ‘5’, ‘5’, ‘3’, ‘5’, 0};
char v10 = {‘6’, ‘5’, ‘5’, ‘3’, ‘5’, 0};

// calculated particle measurement values
unsigned int PM25, PM100;

// warning vaues (set to 9999 for no warning)
// these defaults are based on EPA values for 24 hours
unsigned int WPM25 = 35;
unsigned int WPM100 = 150;

int delaymsecs = 6000; // delay for read (min is <6000 from data sheet)
// testing indicates 2 seconds will work

//--------------------------------------------------------------

void setup(void) {
// Note: Upon power up, the HPM defaults to Fan and Auto Send On
u8g2.begin();
HWSERIAL.begin(9600, SERIAL_8N1);
// display template on OLED
u8g2.clearBuffer(); // clear OLED memory
u8g2.setFont(u8g2_font_10x20_me);
u8g2.drawStr(0, 14, s25);
u8g2.drawStr(70, 14, “9999”);
u8g2.drawStr(0, 31, s100);
u8g2.drawStr(70, 31, “9999”);
u8g2.sendBuffer(); // transfer internal memory to the display
delay(1000);
// stop HPM default auto send
while (StopAuto() == false) {
// display error
u8g2.drawStr(118, 14, “E”); // display warning
u8g2.sendBuffer();
u8g2.drawStr(118, 31, “E”); // display warning
u8g2.sendBuffer();
}
// clear error on recovery
u8g2.drawStr(118, 14, " "); // clear warning
u8g2.sendBuffer();
u8g2.drawStr(118, 31, " "); // clear warning
u8g2.sendBuffer();
delay(5000); // initial settle
}

void loop(void) {

if (ReadPM() == false) {
// display read error (‘e’)
u8g2.drawStr(118, 14, “e”); // display warning
u8g2.sendBuffer();
u8g2.drawStr(118, 31, “e”); // display warning
u8g2.sendBuffer();
// note: ‘e’ will be cleared on a succesful read below
}
else {
// display PM values on the OLED
// note: we only display 1000s down as
// per max value from data sheet
conPM(PM25, v25);
u8g2.drawStr(70, 14, &v25[1]);
u8g2.sendBuffer();
conPM(PM100, v10);
u8g2.drawStr(70, 31, &v10[1]);
u8g2.sendBuffer();
if (PM25 > WPM25) {
u8g2.drawStr(118, 14, “"); // display warning
u8g2.sendBuffer();
}
else {
u8g2.drawStr(118, 14, " "); // clear warning
u8g2.sendBuffer();
}
if (PM100 > WPM100) {
u8g2.drawStr(118, 31, "
”); // display warning
u8g2.sendBuffer();
}
else {
u8g2.drawStr(118, 31, " "); // clear warning
u8g2.sendBuffer();
}
}
// wait 6 seconds (default) before sending the next request
delay(delaymsecs);
}

void conPM(int PMvalue, char *charArray) {
// convert the particle measures to char array storage for display
// with leading zeros. Note: no error checking!
byte digit = 0;
bool LZ = true;

for (int div = 10000, mod = 0; div > 0; div /= 10) {
mod = PMvalue % div;
PMvalue /= div;
if (!LZ || PMvalue != 0) {
LZ = false;
charArray[digit++] = PMvalue + ‘0’;
}
else {
charArray[digit++] = ‘0’;
}
PMvalue = mod;
}
charArray[digit] = 0; // zero delimiter
}

bool StopAuto(void) {
// attempt to stop the default power up auto send
// if it fails, an ‘E’ will be displayed on the screen
// in a terminal loop until a successful transmission
// Note: this should never happen an indicates a
// chronic serial transmission error.
byte maxattempts = 2; // set this as you like
byte attempts;
int R1 = 0, R2 = 0;

// wait until there is a pause in Auto-Send
while (HWSERIAL.available()>0)
{
HWSERIAL.read();
}
attempts = 0;
bool result = false;
while (attempts < maxattempts) {
HWSERIAL.clear();
// send a stop auto send
HWSERIAL.write(0x68);
HWSERIAL.write(0x01);
HWSERIAL.write(0x20);
HWSERIAL.write(0x77);
// read response
delay(25);
// if there are no bytes ready, -1 is returned
R1 = HWSERIAL.read();
R2 = HWSERIAL.read();
if ( (R1 != 0xA5) || (R2 != 0xA5) ) {
attempts++;
}
else {
attempts = maxattempts; // force an exit
result = true; // with an ok return
}
}
return (result);
}

bool ReadPM(void) {
// send a read request and get the particle measures in the response
// if we don’t get the bytes back or we have a checksum error
// return false - else return true

byte maxattempts = 2; // set this as you like
byte attempts = 0; // attempt counter
byte R[8]; // response bytes
int nbytes; // byte counter
unsigned long ck; // for checksum calculation
bool result = false;

while (attempts < maxattempts) {
// send a read request send
HWSERIAL.clear();
HWSERIAL.write(0x68);
HWSERIAL.write(0x01);
HWSERIAL.write(0x04);
HWSERIAL.write(0x93); //cksum
delay(25);
// we want to read 8 bytes of data
// Pos ACK - 0x40,0x05,0x04
// DF1,DF2 - 2.5 high / low
// DF1,DF2 - 10.0 high / low
// Cksum (mod 256)
nbytes = 0;
ck = 0;
while (HWSERIAL.available() && nbytes < 8) {
R[nbytes++] = HWSERIAL.read(); // store the byte
}
if (nbytes == 8) {
// we got them and R[7] hold the checksum
ck = ((65536 - (R[0] + R[1] + R[2] + R[3] + R[4] + R[5] + R[6])) & 255);
if (ck == R[7]) {
// everything looks good so calculate the global particle measures
PM25 = (R[3] * 256) + R[4];
PM100 = (R[5] * 256) + R[6];
attempts = maxattempts;
result = true;
}
else {
//checksum error
attempts++;
}
}
else {
// serial tx error [not enough bytes]
attempts++;
}
}
return (result);
}