void rgltrRunController(void) {
unsigned long time;
float steer, thrust;
PoseEstimateStruct pose;
if(!is_ready) { return; } // Don't run if not ready
if(reg_state == REG_OFF) { return; } // Don't run if not running
time = sclockGetGlobalTicks(); // Record system time
steer = 0.0;
thrust = 0.0;
pose.yaw = 0.0; // Initialize to make optimizer happy
pose.pitch = 0.0;
if(reg_state == REG_REMOTE_CONTROL) {
steer = rc_outputs.steer;
thrust = rc_outputs.thrust;
} else if(reg_state == REG_TRACK){
steer = runYawControl(pose.yaw);
thrust = runPitchControl(pose.pitch);
// ? = runRollControl(roll); // No roll actuator yet
}
// TODO: Roll control mixing
mcSteer(steer);
mcThrust(thrust);
}
static void cmdSteer(unsigned char status, unsigned char length, unsigned char *frame) {
unsigned char chr_test[4];
float *steer_value = (float*) chr_test;
chr_test[0] = frame[0];
chr_test[1] = frame[1];
chr_test[2] = frame[2];
chr_test[3] = frame[3];
mcSteer(steer_value[0]);
}
