void loop() { static float yaw_target = 0; // Wait until new orientation data (normally 5ms max) while (ins.num_samples_available() == 0); static uint32_t lastPkt = 0; static int16_t channels[8] = {0,0,0,0,0,0,0,0}; Iriss::Command doSomething = check_for_command(); do_respond(doSomething, channels); // turn throttle off if no update for 0.5seconds if(hal.scheduler->millis() - lastPkt > 500) { channels[2] = 0; } long rcthr, rcyaw, rcpit, rcroll; // Variables to store radio in // Read RC transmitter rcthr = channels[2]; rcyaw = channels[3]; rcpit = channels[1]; rcroll = channels[0]; // Ask MPU6050 for orientation ins.update(); float roll,pitch,yaw; ins.quaternion.to_euler(&roll, &pitch, &yaw); roll = ToDeg(roll) ; pitch = ToDeg(pitch) ; yaw = ToDeg(yaw) ; // Ask MPU6050 for gyro data Vector3f gyro = ins.get_gyro(); float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z); // Do the magic if(rcthr > RC_THR_MIN + 100) { // Throttle raised, turn on stablisation. // Stablise PIDS float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250); float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250); float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360); // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output) if(abs(rcyaw ) > 5) { yaw_stab_output = rcyaw; yaw_target = yaw; // remember this yaw for when pilot stops } // rate PIDS long pitch_output = (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500); long roll_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500); long yaw_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500); // mix pid outputs and send to the motors. hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output); hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output); hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output); hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output); } else { // motors off hal.rcout->write(MOTOR_FL, 1000); hal.rcout->write(MOTOR_BL, 1000); hal.rcout->write(MOTOR_FR, 1000); hal.rcout->write(MOTOR_BR, 1000); // reset yaw target so we maintain this on takeoff yaw_target = yaw; // reset PID integrals whilst on the ground for(int i=0; i<6; i++) { pids[i].reset_I(); } } }
void do_respond(const Iriss::Command& cmd, int16_t *channels) { uint32_t directives = cmd.get(); if(directives == Iriss::Command::TX_ERR) { return; } if(directives & Iriss::Command::ACK) { // reply with an ACK if not waiting on one if(!piOnline) { Iriss::Command resp(Iriss::Command::ACK); hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1); uint8_t lenBuf[4]; Utils::Packable::pack_uint32(Iriss::Command::PACKED_SIZE, lenBuf); hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE)); uint8_t cmdBuf[Iriss::Command::PACKED_SIZE]; resp.pack(cmdBuf, Iriss::Command::PACKED_SIZE); hal.console->write(cmdBuf, Iriss::Command::PACKED_SIZE); piOnline = true; } } if(directives & Iriss::Command::SEND_AGAIN) { // send the last command Iriss::Command resp; resp.set(lastSent); hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1); uint8_t lenBuf[4]; Utils::Packable::pack_uint32(Iriss::Command::PACKED_SIZE, lenBuf); hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE)); uint8_t cmdBuf[Iriss::Command::PACKED_SIZE]; resp.pack(cmdBuf, Iriss::Command::PACKED_SIZE); hal.console->write(cmdBuf, Iriss::Command::PACKED_SIZE); } if(directives & Iriss::Command::GET_ORIENTATION) { // get the orientation from ins and send it ins.update(); float roll,pitch,yaw; ins.quaternion.to_euler(&roll, &pitch, &yaw); roll = ToDeg(roll); pitch = ToDeg(pitch); yaw = ToDeg(yaw); Iriss::Orientation orientation(roll, pitch, yaw); hal.console->write(&Iriss::BEGIN_UART_MESSAGE, 1); uint8_t lenBuf[4]; Utils::Packable::pack_uint32(Iriss::Orientation::PACKED_SIZE, lenBuf); hal.console->write(lenBuf, sizeof(Iriss::Command::PACKED_SIZE)); uint8_t orientBuf[Iriss::Orientation::PACKED_SIZE]; orientation.pack(orientBuf, Iriss::Orientation::PACKED_SIZE); hal.console->write(orientBuf, Iriss::Orientation::PACKED_SIZE); } if(directives & Iriss::Command::NUDGE_ROLL_LEFT) { // update channels 0 --channels[0]; } if(directives & Iriss::Command::NUDGE_ROLL_RIGHT) { // update channels 0 in the other way from LEFT ++channels[0]; } if(directives & Iriss::Command::NUDGE_UP) { // update channels 2 ++channels[2]; } if(directives & Iriss::Command::NUDGE_DOWN) { // update channels 2 in the other way from UP --channels[2]; } if(directives & Iriss::Command::NUDGE_YAW_CCW) { // update channels 3 ++channels[3]; } if(directives & Iriss::Command::NUDGE_YAW_CW) { // update channels 3 in the other way from CCW --channels[3]; } if(directives & Iriss::Command::NUDGE_PITCH_DOWN) { // update channels 1 ++channels[1]; } if(directives & Iriss::Command::NUDGE_PITCH_UP) { // update channels 1 in the other way from DOWN --channels[1]; } }