Custom Flight mode, Guidance System


First off I hope I’m in the right place here, I read around and searched a bit and it seemed allright.

I’m trying to implement a guidance system that takes an analog input, and changes the desired pitch. I figured to start with I’ll just modify the stabilize algorithm. Here are the additions I’ve made. And then I tried testing it by taking off the props, and running it at min throttle, and using an arduino pwm signal to make a signal for the analog input. Perhaps somebody could tell me the terrible mistakes I’m making? Thanks for any help in advance!

//kd Analog In, setup, may not be complete
AP_HAL::AnalogSource* ch1;
AP_HAL::AnalogSource* ch2;
AP_HAL::AnalogSource* ch3;

// the below is in the stabilize run function
//kd adc variables  conv factor is 3 since lean angle is in centi degrees so a 30 degree lean would be 3000, and the adc pulls in max 1023, so 3*1023 would give a lean angle of 30 degrees
float adcroll = 0, adcpitch = 0, criticalfactor = 1, convfactor = 3;
ch1 = hal.analogin->channel(15);
ch2 = hal.analogin->channel(13);
ch3 = hal.analogin->channel(14);

//kd should just change target_roll, and target_pitch with adc input  (need to know a good scaling factor). 
adcpitch = ch1->voltage_average(); 
target_pitch = target_pitch + adcpitch*convfactor*criticalfactor;
//target_roll = target_roll + adcroll*cfactor;