void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
{
// Thrust
uint16_t rawThrust = commanderGetActiveThrust();
if (thrustLocked || (rawThrust < MIN_THRUST)) {
setpoint->thrust = 0;
} else {
setpoint->thrust = min(rawThrust, MAX_THRUST);
}
if (altHoldMode) {
setpoint->thrust = 0;
setpoint->mode.z = modeVelocity;
setpoint->velocity.z = ((float) rawThrust - 32767.f) / 32767.f;
} else {
setpoint->mode.z = modeDisable;
}
// roll/pitch
if (posHoldMode) {
setpoint->mode.x = modeVelocity;
setpoint->mode.y = modeVelocity;
setpoint->mode.roll = modeDisable;
setpoint->mode.pitch = modeDisable;
setpoint->velocity.x = commanderGetActivePitch()/30.0f;
setpoint->velocity.y = commanderGetActiveRoll()/30.0f;
setpoint->attitude.roll = 0;
setpoint->attitude.pitch = 0;
} else {
setpoint->mode.x = modeDisable;
setpoint->mode.y = modeDisable;
setpoint->mode.roll = modeAbs;
setpoint->mode.pitch = modeAbs;
setpoint->velocity.x = 0;
setpoint->velocity.y = 0;
setpoint->attitude.roll = commanderGetActiveRoll();
setpoint->attitude.pitch = commanderGetActivePitch();
}
// Yaw
setpoint->attitudeRate.yaw = commanderGetActiveYaw();
yawModeUpdate(setpoint, state);
setpoint->mode.yaw = modeVelocity;
}
void crtpCommanderRpytDecodeSetpoint(setpoint_t *setpoint, CRTPPacket *pk)
{
struct CommanderCrtpLegacyValues *values = (struct CommanderCrtpLegacyValues*)pk->data;
if (commanderGetActivePriority() == COMMANDER_PRIORITY_DISABLE) {
thrustLocked = true;
}
if (values->thrust == 0) {
thrustLocked = false;
}
// Thrust
uint16_t rawThrust = values->thrust;
if (thrustLocked || (rawThrust < MIN_THRUST)) {
setpoint->thrust = 0;
} else {
setpoint->thrust = min(rawThrust, MAX_THRUST);
}
if (altHoldMode) {
setpoint->thrust = 0;
setpoint->mode.z = modeVelocity;
setpoint->velocity.z = ((float) rawThrust - 32767.f) / 32767.f;
} else {
setpoint->mode.z = modeDisable;
}
// roll/pitch
if (posHoldMode) {
setpoint->mode.x = modeVelocity;
setpoint->mode.y = modeVelocity;
setpoint->mode.roll = modeDisable;
setpoint->mode.pitch = modeDisable;
setpoint->velocity.x = values->pitch/30.0f;
setpoint->velocity.y = values->roll/30.0f;
setpoint->attitude.roll = 0;
setpoint->attitude.pitch = 0;
} else if (posSetMode && values->thrust != 0) {
setpoint->mode.x = modeAbs;
setpoint->mode.y = modeAbs;
setpoint->mode.z = modeAbs;
setpoint->mode.roll = modeDisable;
setpoint->mode.pitch = modeDisable;
setpoint->mode.yaw = modeAbs;
setpoint->position.x = -values->pitch;
setpoint->position.y = values->roll;
setpoint->position.z = values->thrust/1000.0f;
setpoint->attitude.roll = 0;
setpoint->attitude.pitch = 0;
setpoint->attitude.yaw = values->yaw;
setpoint->thrust = 0;
} else {
setpoint->mode.x = modeDisable;
setpoint->mode.y = modeDisable;
if (stabilizationModeRoll == RATE) {
setpoint->mode.roll = modeVelocity;
setpoint->attitudeRate.roll = values->roll;
setpoint->attitude.roll = 0;
} else {
setpoint->mode.roll = modeAbs;
setpoint->attitudeRate.roll = 0;
setpoint->attitude.roll = values->roll;
}
if (stabilizationModePitch == RATE) {
setpoint->mode.pitch = modeVelocity;
setpoint->attitudeRate.pitch = values->pitch;
setpoint->attitude.pitch = 0;
} else {
setpoint->mode.pitch = modeAbs;
setpoint->attitudeRate.pitch = 0;
setpoint->attitude.pitch = values->pitch;
}
setpoint->velocity.x = 0;
setpoint->velocity.y = 0;
}
// Yaw
if (!posSetMode) {
setpoint->attitudeRate.yaw = values->yaw;
yawModeUpdate(setpoint);
setpoint->mode.yaw = modeVelocity;
}
}
