static numvar ioCmd() {
byte n = getarg(0);
if(!n) {
ioSetup();
}else if(n==1) {
switch(getarg(1)) {
case 0:
n = Body::mcp_.readGPIO(0);
#define PRINT_BIT(_b) (n&(1<<_b))?spb('1'):spb('0')
PRINT_BIT(PIN_FRAME_STOP);
PRINT_BIT(PIN_STEP_STOP1);
PRINT_BIT(PIN_STEP_STOP2);
speol();
break;
case 1:
n = Body::mcp_.readGPIO(1);
printHex(n);
speol();
break;
case 2:
printHex(button_);
speol();
button_ = 0xff;
break;
}
}else{
switch(getarg(1)) {
case 0:
powerSetup(PIN_EP_EN,getarg(2));
break;
}
}
return 0;
}
int main(void) {
halInit();
chSysInit();
sdStart(&SD1, &serialConfig);
pilotSetup();
powerSetup();
motorsSetup();
imuSetup();
pidInit(&pitchPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&rollPID, 1.0 / 20000.0, 1.0 / 10000, 0);
pidInit(&yawPID, 1.0 / 20000.0, 1.0 / 10000, 0);
while (TRUE) {
printf("yaw rate %f\r\n", imuGetYawRate());
printf("pitch %f\r\n", imuGetPitch());
printf("roll %f\r\n", imuGetRoll());
// double pitch = pilotGetPitch() - pidUpdate(&pitchPID, 0.0, gyroGetPitchRotation());
// double roll = pilotGetRoll() - pidUpdate(&rollPID, 0, gyroGetRollRotation());
// double yaw = pilotGetYaw() - pidUpdate(&yawPID, 0, gyroGetYawRotation());
// motorsSetControl(CLIP(pitch), CLIP(roll), CLIP(yaw), pilotGetThrottle());
// uint16_t throttle = receiverGetRaw(THROTTLE_CH);
// uint16_t pitch = receiverGetRaw(PITCH_CH);
// uint16_t roll = receiverGetRaw(ROLL_CH);
// uint16_t yaw = receiverGetRaw(YAW_CH);
// printf("%u, %u, %u, %u\r\n", throttle, pitch, roll, yaw);
// printf("%u\r\n", receiverGetRaw(4));
// printf("%f, %f, %f\r\n", receiverGetDouble(PITCH_CH), receiverGetDouble(ROLL_CH), receiverGetDouble(YAW_CH));
// printf("%d, %d, %d\r\n", gyroGetPitchRotation(), gyroGetRollRotation(), gyroGetYawRotation());
// printf("%f, %f, %f\r\n", pitch, roll, yaw);
// printf("%f, %f, %f\r\n", pitchPID.integral, rollPID.integral, yawPID.integral);
chThdSleepSeconds(1);
}
}
