patrickelectric

#include <ArduLink.h>
ArduLink simulink;

patrickelectric

/// DEBUG
		#if 1
	    	// multwii
	    	#if 1

		    	c_common_datapr_multwii_bicopter_identifier();
		    	c_common_datapr_multwii_motor_pins();
		    	c_common_datapr_multwii_motor(iActuation.escLeftSpeed+2,iActuation.escRightSpeed+2);
		    	c_common_datapr_multwii_attitude(rpy[PV_IMU_ROLL  ]*RAD_TO_DEG, rpy[PV_IMU_PITCH  ]*RAD_TO_DEG, rpy[PV_IMU_YAW  ]*RAD_TO_DEG );
		    	arm_scale_f32(accRaw,RAD_TO_DEG,accRaw,3);

patrickelectric

void  EXTI9_5_IRQHandler()
{

	EXTI_ClearITPendingBit(EXTI_LINE); // clear interrupt
	int pulse_width = TIM_GetCounter(TIM2) - PULSE_INTERVAL;

	if(pulse_width > SYNC_WIDTH) //sync pulse
		channel_index = 0;
	else {
		if(channel_index < NUM_OF_CHANNELS) {

patrickelectric

/// IMU DATA
		#if 1
			
		 	c_common_gpio_toggle(LED_builtin_io);
		 	c_io_imu_getRaw(accRaw, gyrRaw, magRaw);
			c_datapr_MahonyAHRSupdate(attitude_quaternion, velAngular, gyrRaw[0],gyrRaw[1],gyrRaw[2],accRaw[0],accRaw[1],accRaw[2],magRaw[0],magRaw[1],magRaw[2]);
			//c_datapr_MahonyAHRSupdate(attitude_quaternion,gyrRaw[0],gyrRaw[1],gyrRaw[2],accRaw[0],accRaw[1],accRaw[2],0,0,0);
			c_io_imu_Quaternion2Euler(attitude_quaternion, rpy);
			c_io_imu_EulerMatrix(rpy, velAngular);
			c_common_gpio_toggle(LED_builtin_io);

patrickelectric

      _____________  /\   ____________
     /\_  _ __ ___ \ \/ / __ _ _ __ / |_
    / /_)/ '__/ _ \ \  / / _` | '_ \| __|
   / ___/| | | (_) \ \/ / (_| | | | | |_
   \/    |_|  \___/ \__/ \__,_|_| |_|\__|

patrickelectric

// must be called in:
// slave tx event callback
// or after beginTransmission(address)
size_t TwoWire::write(const uint8_t *data, size_t quantity)
{
  if(transmitting){
  // in master transmitter mode
    for(size_t i = 0; i < quantity; ++i){
      write(data[i]);
    }

patrickelectric

/* 
 * Function twi_transmit
 * Desc     fills slave tx buffer with data
 *          must be called in slave tx event callback
 * Input    data: pointer to byte array
 *          length: number of bytes in array
 * Output   1 length too long for buffer
 *          2 not slave transmitter
 *          0 ok
 */

patrickelectric

#! /usr/bin/env python
# -*- coding:utf-8 -*-
# 
__author__ = 'Patrick'


import matplotlib.pyplot as plt
import matplotlib.animation as animation
import time
import datetime

patrickelectric

#! /usr/bin/env python
# -*- coding:utf-8 -*-
# 
__author__ = 'Patrick'


import matplotlib.pyplot as plt
import matplotlib.animation as animation
import time
import datetime

patrickelectric

figure;  
h = plot3(0,0,0);
p = get(h,'Parent');
xlim(p,'manual');  
xlim(p,[-2 2]);
ylim(p,'manual');
ylim(p,[-2 2]);
zlim(p,'manual');
zlim(p,[-2 2]);
axis vis3d;grid on;