/* map pid to motors 4 + ----------------------------------------------------*/
void sendPIDMotors4Plus(vs32 * PIDCorr, vu16 * receiverChannel, volatile char * motor)
{
// no hal used at the moment
s16 motorTemp[13];
// map pitch to quax blmc values from 0-200
u8 pitch = (receiverChannel[PITCH] - 1000) / 5;
motorTemp[1] = pitch - PIDCorr[Y] - PIDCorr[Z];
motorTemp[2] = pitch + PIDCorr[Y] - PIDCorr[Z];
motorTemp[3] = pitch + PIDCorr[X] + PIDCorr[Z];
motorTemp[4] = pitch - PIDCorr[X] + PIDCorr[Z];
motorTemp[5] = 0;
motorTemp[6] = 0;
motorTemp[7] = 0;
motorTemp[8] = 0;
motorTemp[9] = 0;
motorTemp[10] = 0;
motorTemp[11] = 0;
motorTemp[12] = 0;
u8 i;
for (i = 1; i < 13; i++)
{
if ((motorTemp[i] - motorTempOld[i]) > 2)
{
motorTemp[i] = motorTempOld[i] + 2;
}
if ((motorTempOld[i] - motorTemp[i]) > 2)
{
motorTemp[i] = motorTempOld[i] - 2;
}
motorTempOld[i] = motorTemp[i];
}
limitMotors(motorTemp, motor);
// command motors
sendMotor(motor);
//char x [80];
//sprintf(x,"mot:%d:%d:%d:%d\r\n",min,max,receiverChannel[PITCH],pitch);
//print_uart1(x);
}
/* map pid to motors 4 X ----------------------------------------------------*/
void sendPIDMotorsMixer(vs32 * PIDCorr, vu16 * receiverChannel, volatile char * motor)
{
// no hal used at the moment
s16 motorTemp[13];
// map pitch to quax blmc values from 0-180 - 20 points for stability
// does not work
//u8 pitch = map(receiverChannel[PITCH],1000,2000,0,180);
u8 pitch = (receiverChannel[PITCH] - 1000) / 5;
u8 pitchFactor[12];
u8 rollFactor[12];
u8 nickFactor[12];
u8 yawFactor[12];
u8 i;
u8 j;
for (i = 0; i < 12; i++)
{
j = (i < 1);
// PARA_MIXER_MOT1A = 37
pitchFactor[i] = parameter[37 + j] >> 8;
rollFactor[i] = parameter[37 + j] & 255;
nickFactor[i] = parameter[38 + j] >> 8;
yawFactor[i] = parameter[38 + j] & 255;
}
for (i = 0; i < 12; i++)
{
motorTemp[i + 1] = (pitch * pitchFactor[i] / 100) + (PIDCorr[X] * rollFactor[i] / 100) + (PIDCorr[Y] * nickFactor[i] / 100) + (PIDCorr[Y] * yawFactor[i] / 100);
}
limitMotors(motorTemp, motor);
// command motors
sendMotor(motor);
//char x [80];
//sprintf(x,"mot:%d:%d:%d:%d\r\n",min,max,receiverChannel[PITCH],pitch);
//print_uart1(x);
}
int main(void)
{
// Alles TriState
DDRA = 0;
DDRB = 0;
DDRC = 0;
DDRD = 0;
PORTA = 0;
PORTB = 0;
PORTC = 0;
PORTD = 0;
// save power
MCUCR |= _BV(JTD);
PRR |= _BV(PRTWI) | _BV(PRSPI) | _BV(PRADC);
//ADCSRA &= ~_BV(ADEN);
//ACSR |=_BV(ACD);
// Power Led
DDRA|=_BV(7);
PIN_SET(A,7);
sei();
// Init: Subsysteme
uart_init(UART_BAUD_SELECT(BAUD,F_CPU));
stella_init();
// curtain_init();
//sensors_init();
// Init: Variablen
enum stateEnum {
StateCommands,
StateCurtainValue,
StateLedChannel,
StateLedValue
} laststate, state = StateCommands;
uint8_t slowdown_counter = 0;
uint8_t slowdown_counter2 = 0;
uint8_t panic_counter = 0;
uint8_t panic_counter_led = 0;
uint8_t panic_counter_brightness = 0;
uint8_t stella_channel = 0;
enum stella_set_function stella_fading = STELLA_SET_IMMEDIATELY;
uint8_t packetPosition = 0;
unsigned char lastChar, haslastChar = 0;
slowdown_counter = 0;
slowdown_counter2 = 0;
stella_setValue(STELLA_SET_IMMEDIATELY, 0, 0);
stella_process();
wdt_enable(WDTO_250MS);
#define PANIC_THRESHOLD 150
// Hauptprogramm
while (1) {
wdt_reset();
stella_process();
curtain_process();
//sensors_process();
//panic counter
if (++slowdown_counter == 0) {
if (++slowdown_counter2==0) {
if (panic_counter < PANIC_THRESHOLD)
++panic_counter;
}
if (panic_counter == PANIC_THRESHOLD) {
panic_counter = PANIC_THRESHOLD+1;
panic_counter_led = 0;
panic_counter_brightness = 0;
packetPosition = 0;
storeValues();
PIN_SET(A,6);
}
if (panic_counter == PANIC_THRESHOLD+1) {
if (panic_counter_brightness==255) {
panic_counter=PANIC_THRESHOLD+2;
continue;
}
stella_setValue(STELLA_SET_FADE, panic_counter_led, ++panic_counter_brightness);
}
if (panic_counter == PANIC_THRESHOLD+2) {
if (panic_counter_brightness==0) {
panic_counter=PANIC_THRESHOLD+1;
if (++panic_counter_led >= STELLA_CHANNELS)
panic_counter_led = 0;
continue;
}
stella_setValue(STELLA_SET_FADE, panic_counter_led, --panic_counter_brightness);
}
}
uint16_t r = uart_getc();
uint8_t info = r >> 8;
if (r == UART_NO_DATA)
continue;
else if (info!=0) {
PIN_SET(A,6);
PIN_SET(A,7);
packetPosition = 0;
continue;
}
const unsigned char temp = haslastChar ? lastChar : 0;
haslastChar = 1;
lastChar = r & 0xff;
if (lastChar == 0xff && temp == 0xff) { // two proceeding 255-values start a valid packet
packetPosition = 1;
state = StateCommands;
continue;
}
if (!packetPosition)
continue;
// reset panic counter
if (panic_counter) {
if (panic_counter>=110) {
restoreValues();
PIN_CLEAR(A,6);
}
panic_counter = 0;
}
laststate = state;
switch (state) {
case StateCommands:
switch (lastChar) {
case 0xef: // get values
sendInit();
sendStella();
//sendSensor();
sendMotor();
break;
case 0xdf: // prepare for a curtain value
state = StateCurtainValue;
break;
case 0xcf: // prepare for a led channel and value (fade:immediately)
state = StateLedChannel;
stella_fading = STELLA_SET_IMMEDIATELY;
break;
case 0xbf: // prepare for a led channel and value (fade:fade)
state = StateLedChannel;
stella_fading = STELLA_SET_FADE;
break;
case 0xaf: // prepare for a led channel and value (fade:fade flashy)
state = StateLedChannel;
stella_fading = STELLA_SET_FLASHY;
break;
case 0x9f: // prepare for a led channel and value (fade:fade immediately+relative)
state = StateLedChannel;
stella_fading = STELLA_SET_IMMEDIATELY_RELATIVE;
break;
case 0x00: // reset panic counter and acknowdlegde
sendAck();
break;
}
break;
case StateCurtainValue:
state = StateCommands;
curtain_setPosition(lastChar);
break;
case StateLedChannel:
state = StateLedValue;
stella_channel = lastChar;
break;
case StateLedValue:
state = StateCommands;
stella_setValue(stella_fading, stella_channel, lastChar);
break;
}
// the state has not changed -> a full packet has been received -> prepare for the next one
if (laststate==state) {
packetPosition=0;
}
}
return 0;
}
