static void sensors_debug() { static int divider = 0; if (divider++ == 20) { divider = 0; float heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix()); hal.console->printf("ahrs: roll %4.1f pitch %4.1f " "yaw %4.1f hdg %.1f\r\n", ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch), ToDeg(g_ahrs.yaw), g_compass.use_for_yaw() ? ToDeg(heading):0.0f); Vector3f accel(g_ins.get_accel()); Vector3f gyro(g_ins.get_gyro()); hal.console->printf("mpu6000: accel %.2f %.2f %.2f " "gyro %.2f %.2f %.2f\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z); hal.console->printf("compass: heading %.2f deg\r\n", ToDeg(g_compass.calculate_heading(0, 0))); } }
void main_task(void *args) { vTaskSetApplicationTaskTag(xTaskGetIdleTaskHandle(), (pdTASK_HOOK_CODE)1); vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)2); led_init(); hal.init(0, NULL); hal.console->printf("AP_HAL Sensor Test\r\n"); hal.scheduler->register_timer_failsafe(failsafe, 1000); hal.console->printf("init AP_InertialSensor: "); g_ins.init(AP_InertialSensor::COLD_START, INS_SAMPLE_RATE, flash_leds); g_ins.init_accel(flash_leds); hal.console->println(); led_set(0, false); hal.console->printf("done\r\n"); hal.console->printf("init AP_Compass: "******"done\r\n"); hal.console->printf("init AP_Baro: "); g_baro.init(); g_baro.calibrate(); hal.console->printf("done\r\n"); hal.console->printf("init AP_AHRS: "); g_ahrs.init(); g_ahrs.set_compass(&g_compass); g_ahrs.set_barometer(&g_baro); hal.console->printf("done\r\n"); portTickType last_print = 0; portTickType last_compass = 0; portTickType last_wake = 0; float heading = 0.0f; last_wake = xTaskGetTickCount(); for (;;) { // Delay to run this loop at 100Hz. vTaskDelayUntil(&last_wake, 10); portTickType now = xTaskGetTickCount(); if (last_compass == 0 || now - last_compass > 100) { last_compass = now; g_compass.read(); g_baro.read(); heading = g_compass.calculate_heading(g_ahrs.get_dcm_matrix()); } g_ahrs.update(); if (last_print == 0 || now - last_print > 100) { last_print = now; hal.console->write("\r\n"); hal.console->printf("ahrs: roll %4.1f pitch %4.1f yaw %4.1f hdg %.1f\r\n", ToDeg(g_ahrs.roll), ToDeg(g_ahrs.pitch), ToDeg(g_ahrs.yaw), g_compass.use_for_yaw() ? ToDeg(heading) : 0.0f); Vector3f accel(g_ins.get_accel()); Vector3f gyro(g_ins.get_gyro()); hal.console->printf("mpu6000: accel %.2f %.2f %.2f " "gyro %.2f %.2f %.2f\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z); hal.console->printf("compass: heading %.2f deg\r\n", ToDeg(g_compass.calculate_heading(0, 0))); g_compass.null_offsets(); if (hal.rcin->valid()) { uint16_t ppm[PPM_MAX_CHANNELS]; size_t count; count = hal.rcin->read(ppm, PPM_MAX_CHANNELS); hal.console->write("ppm: "); for (size_t i = 0; i < count; ++i) hal.console->printf("%u ", ppm[i]); hal.console->write("\r\n"); } } } }
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(); } } }