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]); }