void start_gyro_calibration(void)
{
if (calibration_enter())
{
// Calibration routine
gyro_calibrate();
calibration_exit();
}
}
void CS_arm(){ // arm through tablet
if(!GPS.fix) return; // POP an error of GPS not fixed, if armed from tablet
homealt = nav.Z + homealt;
#ifdef HIL_MODE
#else
gyro_calibrate();
#endif
RCmode.arm = 1;
armcount = ARMCNTNO;
tablet_armed = 1;
}
mraa_result_t gyro_init(int bus) {
//init bus and reset chip
gyro_context = mraa_i2c_init(bus);
mraa_i2c_address(gyro_context, ITG3200_I2C_ADDR);
mraa_result_t result;
gyro_buffer[0] = ITG3200_PWR_MGM;
gyro_buffer[1] = ITG3200_RESET;
result = mraa_i2c_write(gyro_context, gyro_buffer, 2);
if (result != MRAA_SUCCESS) {
printError("unable to write to gyro (4)");
return result;
}
gyro_calibrate();
gyro_update();
return MRAA_SUCCESS;
}
void _protocol_dispatch(uint8_t cmd, uint8_t length) {
status_set(STATUS_MESSAGE_RX);
switch(cmd) {
case 'A':
for (uint8_t i = 0; i < 4; i++) {
_last_flight_val[i] = (_buf)[i];
}
_last_flight_cmd = cmd;
break;
case 'M':
_last_flight_cmd = cmd;
break;
case 'C':
gyro_calibrate();
accel_calibrate();
protocol_send_diag("calibrated");
break;
case 't':
pid_send_tuning();
attitude_send_tuning();
protocol_send_diag("tuning sent");
break;
case 'E':
_telemetry_enabled = _telemetry_enabled ? 0x00 : 0x01;
break;
case 'r':
_raw_enabled = _raw_enabled ? 0x00 : 0x01;
break;
case 'p':
pid_receive_tuning(_buf);
protocol_send_diag("pid received");
break;
case 'c':
case 'k':
if (attitude_get_id() == 'K') {
attitude_receive_tuning(_buf);
protocol_send_diag("kalman received");
} else if (attitude_get_id() == 'C') {
attitude_receive_tuning(_buf);
protocol_send_diag("complementary received");
}
break;
}
}
void sticks(){
static int index=0;
static float gain = 0;
static int tempthrottle = 0;
rc.roll = SAILERON; // RC channels defined
rc.pitch = SELEVATOR;
rc.yaw = SRUDDER;
rc.throttle = STHROTTLE;
// if(rc.throttle<800)return; // this line will disable all further calculations if there is no RC receiver connected
if(Shutdown == 1 && RCmode.arm){ // disarm motors on shutdown... relocate this code to control_system on removal of sticks
RCmode.arm = 0;
tablet_armed = 0;
armcount = 0;
Shutdown = 0;
RCmode.alt = 0;
RCmode.gps = 0;
RCmode.nav = 0;
rc.throttle = *MOTOR_MIN-50;
}
// if(POT_CTRLR>1500 || STHROTTLE<800){ // comment these lines for removing tablet control ability for channel 6
// rc.throttle = *MOTOR_MIN - 5;
// return;
// }
tablet_armed = 0;
if(rc.throttle< 1070 && rc.throttle>900){
Shutdown = 0;
if(rc.yaw > 1800 && rc.yaw < 2050 && !RCmode.arm) armcount++;
else if(rc.yaw < 1200 && rc.yaw>950 && RCmode.arm) armcount--;
if(armcount>=ARMCNTNO && !RCmode.arm){ // manual arming through RC
RCmode.arm = 1;
tablet_armed = 0;
homealt = nav.Z + homealt;
armcount = ARMCNTNO;
#ifdef HIL_MODE
#else
gyro_calibrate();
#endif
}
else{
if(armcount<=0 && RCmode.arm){
RCmode.arm = 0;
tablet_armed = 0;
armcount = 0;
}
}
}
index = *RC_PARAM_IDXL;
if(index>0 && index<12) { // 12 number of PIDs
gain = *RC_PARAM_MINL + (POT_CTRLL-1000)*(*RC_PARAM_MAXL - *RC_PARAM_MINL)/1000.0f;
if(gain>*RC_PARAM_MAXL) gain = *RC_PARAM_MAXL;
else if(gain<*RC_PARAM_MINL) gain = *RC_PARAM_MINL;
global_data.param[index-1] = gain;
}
// if(POT_CTRLR>1500 && !RCmode.nav)trigger(5000);
// if(POT_CTRLR>1500){
// Takeoff = 1;
// Landing = 1;
//
//
// }
index = (int)*RC_PARAM_IDXR;
if(index>0 && index<=12) { // 12 number of PIDs
gain = *RC_PARAM_MINR + (POT_CTRLR-1000)*(*RC_PARAM_MAXR - *RC_PARAM_MINR)/1000.0f;
if(gain>*RC_PARAM_MAXR) gain = *RC_PARAM_MAXR;
else if(gain<*RC_PARAM_MINR) gain = *RC_PARAM_MINR;
global_data.param[index-1] = gain;
}
if(abs(rc.yaw-1500)>YAWDEADBAND && rc.throttle>*MOTOR_MIN){
cstarget.yaw = cstarget.yaw + (*RC_YSENSE)*(rc.yaw - 1500)/1000.0f;
if(cstarget.yaw>180)cstarget.yaw -=360;
if(cstarget.yaw<=-180)cstarget.yaw+=360;
}
if(RCmode.alt!=OFF){
tempthrottle = rc.throttle - HoverStickValue;
switch(RCmode.alt){
case ALT_HOLD: {
if(abs(tempthrottle)>ALTDEADBAND)targetnav.Z += (*RC_ASENSE)*(tempthrottle)/20000.0f; // 12.5 cm/s to 1.25 m/s for 50 to 500 us
}
break;
case VV_CONTROL:{
if(abs(tempthrottle)>ALTDEADBAND){
vv_target= (*RC_ASENSE)*(tempthrottle)/200.0f; //
}
else{
vv_target = 0;
}
}
default: break;
}
}
else {
HoverStickValue = rc.throttle;
}
}
