/* 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; }