Come da titolo qualcuno mi può aiutare ad installare Megapirate sulla Flyduino V2 con FreeImu 0.3.5MS?
Io ci sto provando ma non ne vengo a capo...
1°
-Scaricato MEGAPIRATE 2.0.49 beta5 (BARO MS5611 Preview)
-Scaricato APM Mission Planner 1.0.82 (altrimenti avevo problemi)
-Scaricato ARDUINO 0022
2°
-Sostituito la cartella "libraries" di ARDUINO 0022 con quella contenuta in MEGAPIRATE
-Configurato il file APM_Config
-Configurato la FreeIMU nel file AP_ADC_ADS7844.cpp
-Configurato la TX in APM_RC.cpp
-Compilato il codice in ARDUINO 0022 (ok)
-Upload con ARDUINO 0022 (ok)
3°
-Collegato la Flyduino con FTDI-USB
-Collegato la BATTERIA LIPO
-Ponticellato il PIN dedicato CLI a GND per entrare in setup mode
-Connect (atteso qualche secondo)
-Terminale
4°
-Da terminale ho eseguito SETUP
-RADIO ed eseguito la calibrazione stick
-LEVEL ed eseguito la calibrazione degli accellerometri
-COMPASS ma dà solo 2 opzioni [ON,OFF] Quindi COMPASS ON
Compass
----------------------------------------
enabled
-ESC devo ancora farla
-SHOW ma vedo che il MAG non è calibrato e poi credo di avere anche un altro problema (140:IMU_SENSOR_CAL : ?? )
Ora come proseguo?
Io ci sto provando ma non ne vengo a capo...
1°
-Scaricato MEGAPIRATE 2.0.49 beta5 (BARO MS5611 Preview)
-Scaricato APM Mission Planner 1.0.82 (altrimenti avevo problemi)
-Scaricato ARDUINO 0022
2°
-Sostituito la cartella "libraries" di ARDUINO 0022 con quella contenuta in MEGAPIRATE
-Configurato il file APM_Config
codice:
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // Example config file. Take a look at config.h. Any term define there can be overridden by defining it here. #define LED_SEQUENCER DISABLED #define MAX_SONAR_RANGE 400 #define BARO_TYPE BARO_MS5611 /* #define BARO_BMP085 0 #define BARO_MS5611 1 */ // MPNG: AP_COMPASS lib make additional ROTATION_YAW_90 for 5883L mag, so in result we have ROTATION_YAW_270 #define MAG_ORIENTATION ROTATION_YAW_180 #define OSD_PROTOCOL OSD_PROTOCOL_NONE /* OSD_PROTOCOL_NONE OSD_PROTOCOL_SYBERIAN OSD_PROTOCOL_REMZIBI */ // New in 2.0.43, but unused in MegairateNG // MPNG: Piezo uses AN5 pin in ArduCopter, we uses AN5 for CLI switch #define PIEZO DISABLED #define PIEZO_LOW_VOLTAGE DISABLED #define PIEZO_ARMING DISABLED #define GPS_PROTOCOL GPS_PROTOCOL_NMEA /* options: GPS_PROTOCOL_NONE without GPS GPS_PROTOCOL_NMEA GPS_PROTOCOL_SIRF GPS_PROTOCOL_UBLOX GPS_PROTOCOL_IMU GPS_PROTOCOL_MTK GPS_PROTOCOL_HIL GPS_PROTOCOL_MTK16 GPS_PROTOCOL_AUTO auto select GPS GPS_PROTOCOL_UBLOX_I2C GPS_PROTOCOL_BLACKVORTEX */ #define SERIAL0_BAUD 115200 // If one want a wireless modem (like APC220) on the console port, lower that to 57600. Default is 115200 #define SERIAL2_BAUD 38400 // GPS port bps #define SERIAL3_BAUD 57600 // default telemetry BPS rate = 57600 #define CLI_ENABLED ENABLED //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) #define FRAME_CONFIG HEXA_FRAME /* options: QUAD_FRAME TRI_FRAME HEXA_FRAME Y6_FRAME OCTA_FRAME HELI_FRAME */ #define FRAME_ORIENTATION X_FRAME /* PLUS_FRAME X_FRAME V_FRAME */ # define CH7_OPTION CH7_DO_NOTHING /* CH7_DO_NOTHING CH7_SET_HOVER CH7_FLIP CH7_SIMPLE_MODE CH7_RTL CH7_AUTO_TRIM CH7_ADC_FILTER (experimental) */ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress #define ACCEL_ALT_HOLD_GAIN 12.0 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode // See the config.h and defines.h files for how to set this up! // // lets use Manual throttle during Loiter //#define LOITER_THR THROTTLE_MANUAL # define RTL_YAW YAW_HOLD //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 // agmatthews USERHOOKS // the choice of function names is up to the user and does not have to match these // uncomment these hooks and ensure there is a matching function un your "UserCode.pde" file //#define USERHOOK_FASTLOOP userhook_FastLoop(); //#define USERHOOK_50HZLOOP userhook_50Hz(); //#define USERHOOK_MEDIUMLOOP userhook_MediumLoop(); //#define USERHOOK_SLOWLOOP userhook_SlowLoop(); //#define USERHOOK_SUPERSLOWLOOP userhook_SuperSlowLoop(); //#define USERHOOK_INIT userhook_init(); // the choice of includeed variables file (*.h) is up to the user and does not have to match this one // Ensure the defined file exists and is in the arducopter directory #define USERHOOK_VARIABLES "UserVariables.h"
codice:
/*
APM_ADC.cpp - ADC ADS7844 Library for Ardupilot Mega
Total rewrite by Syberian:
Full I2C sensors replacement:
ITG3200, BMA180
Integrated analog Sonar on the ADC channel 7 (in centimeters)
//D10 (PORTL.1) = input from sonar
//D9 (PORTL.2) = sonar Tx (trigger)
//The smaller altitude then lower the cycle time
*/
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
#include <AP_I2C.h>
#include "AP_ADC_ADS7844.h"
//*****************************
// Select your IMU board type:
// #define FFIMU
// #define ALLINONE
#define FREEIMU_35
// #define BMA_020 // do you have it?
// *********************
// I2C general functions
// *********************
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5);
#ifdef ALLINONE
#define BMA180_A 0x82
#else
#define BMA180_A 0x80
#endif
#ifdef BMA_020
#define ACC_DIV 25.812
#else
#define ACC_DIV 28
#endif
int adc_value[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int rawADC_ITG3200[6],rawADC_BMA180[6];
long adc_read_timeout=0;
volatile uint32_t last_ch6_micros;
volatile uint32_t last_accel_read_millis;
// Constructors ////////////////////////////////////////////////////////////////
AP_ADC_ADS7844::AP_ADC_ADS7844()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_ADC_ADS7844::Init(void)
{
int i;
i2c.init();
//=== ITG3200 INIT
delay(10);
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c.rep_start(0xd0+0); // I2C write direction
i2c.write(0x3E); // Power Management register
i2c.write(0x80); // reset device
delay(5);
i2c.rep_start(0xd0+0); // I2C write direction
i2c.write(0x15); // register Sample Rate Divider
i2c.write(0x3+1); // 7: 1000Hz/(3+1) = 250Hz .
delay(5);
i2c.rep_start(0xd0+0); // I2C write direction
i2c.write(0x16); // register DLPF_CFG - low pass filter configuration & sample rate
i2c.write(0x18+4); // Internal Sample Rate 1kHz, 1..6: 1=200hz, 2-100,3-50,4-20,5-10,6-5
delay(5);
i2c.rep_start(0xd0+0); // I2C write direction
i2c.write(0x3E); // Power Management register
i2c.write(0x03); // PLL with Z Gyro reference
delay(100);
delay(10);
#ifndef BMA_020
//===BMA180 INIT
i2c.rep_start(BMA180_A+0); // I2C write direction
i2c.write(0x0D); // ctrl_reg0
i2c.write(1<<4); // Set bit 4 to 1 to enable writing
i2c.rep_start(BMA180_A+0);
i2c.write(0x35);
i2c.write(3<<1); // range set to 3. 2730 1G raw data. With /10 divisor on acc_ADC, more in line with other sensors and works with the GUI
i2c.rep_start(BMA180_A+0);
i2c.write(0x20); // bw_tcs reg: bits 4-7 to set bw
i2c.write(0<<4); // bw to 10Hz (low pass filter)
#else
byte control; // BMA020 INIT
i2c.rep_start(0x70); // I2C write direction
i2c.write(0x15); //
i2c.write(0x80); // Write B10000000 at 0x15 init BMA020
i2c.rep_start(0x70); //
i2c.write(0x14); //
i2c.write(0x71); //
i2c.rep_start(0x71); //
control = i2c.readNak();
control = control >> 5; //ensure the value of three fist bits of reg 0x14 see BMA020 documentation page 9
control = control << 2;
control = control | 0x00; //Range 2G 00
control = control << 3;
control = control | 0x00; //Bandwidth 25 Hz 000
i2c.rep_start(0x70); // I2C write direction
i2c.write(0x14); // Start multiple read at reg 0x32 ADX
i2c.write(control);
#endif
delay(10);
// Sonar INIT
//=======================
//D48 (PORTL.1) = sonar input
//D47 (PORTL.2) = sonar Tx (trigger)
//The smaller altitude then lower the cycle time
// 0.034 cm/micros
//PORTL&=B11111001;
//DDRL&=B11111101;
//DDRL|=B00000100;
PORTH&=B10111111; // H6 -d9 - sonar TX
DDRH |=B01000000;
PORTB&=B11101111; // B4 -d10 - sonar Echo
DDRB &=B11101111;
last_accel_read_millis = 0;
last_ch6_micros = micros();
//PORTG|=B00000011; // buttons pullup
//div64 = 0.5 us/bit
//resolution =0.136cm
//full range =11m 33ms
// Using timer5, warning! Timer5 also share with RC PPM decoder
TCCR5A =0; //standard mode with overflow at A and OC B and C interrupts
TCCR5B = (1<<CS11); //Prescaler set to 8, resolution of 0.5us
TIMSK5=B00000111; // ints: overflow, capture, compareA
OCR5A=65510; // approx 10m limit, 33ms period
OCR5B=3000;
}
// Sonar read interrupts
volatile char sonar_meas=0;
volatile unsigned int sonar_data=0, sonar_data_start=0, pre_sonar_data=0; // Variables for calculating length of Echo impulse
ISR(TIMER5_COMPA_vect) // This event occurs when counter = 65510
{
if (sonar_meas == 0) // sonar_meas=1 if we not found Echo pulse, so skip this measurement
sonar_data = 0;
PORTH|=B01000000; // set Sonar TX pin to 1 and after ~12us set it to 0 (below) to start new measurement
}
ISR(TIMER5_OVF_vect) // Counter overflowed, 12us elapsed
{
PORTH&=B10111111; // set TX pin to 0, and wait for 1 on Echo pin (below)
sonar_meas=0; // Clean "Measurement finished" flag
}
ISR(PCINT0_vect)
{
if (PINB & B00010000) {
sonar_data_start = TCNT5; // We got 1 on Echo pin, remeber current counter value
} else {
sonar_data=TCNT5-sonar_data_start; // We got 0 on Echo pin, calculate impulse length in counter ticks
sonar_meas=1; // Set "Measurement finished" flag
}
}
void i2c_ACC_getADC () { // ITG3200 read data
static uint8_t i;
i2c.rep_start(0XD0); // I2C write direction ITG3200
i2c.write(0X1D); // Start multiple read
i2c.rep_start(0XD0 +1); // I2C read direction => 1
for(i = 0; i< 5; i++) {
rawADC_ITG3200[i]= i2c.readAck();}
rawADC_ITG3200[5]= i2c.readNak();
#ifdef ALLINONE
adc_value[0] = ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]); //g yaw
adc_value[1] = ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]); //g roll
adc_value[2] =- ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]); //g pitch
#endif
#ifdef FREEIMU_35
adc_value[0] = ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]); //g yaw
adc_value[1] = ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]); //g roll
adc_value[2] =- ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]); //g pitch
#endif
#ifdef FFIMU
adc_value[0] = ((rawADC_ITG3200[4]<<8) | rawADC_ITG3200[5]); //g yaw
adc_value[2] = ((rawADC_ITG3200[2]<<8) | rawADC_ITG3200[3]); //g roll
adc_value[1] = ((rawADC_ITG3200[0]<<8) | rawADC_ITG3200[1]); //g pitch
#endif
// Accel updates at 10Hz
if (millis() - last_accel_read_millis >= 100)
{
#ifndef BMA_020
i2c.rep_start(BMA180_A); // I2C write direction BMA 180
i2c.write(0x02); // Start multiple read at reg 0x02 acc_x_lsb
i2c.rep_start(BMA180_A +1); // I2C read direction => 1
for( i = 0; i < 5; i++) {
rawADC_BMA180[i]=i2c.readAck();}
rawADC_BMA180[5]= i2c.readNak();
#else // BMA020
i2c.rep_start(0x70);
i2c.write(0x02);
i2c.write(0x71);
i2c.rep_start(0x71);
for( i = 0; i < 5; i++) {
rawADC_BMA180[i]=i2c.readAck();}
rawADC_BMA180[5]= i2c.readNak();
#endif
#ifdef ALLINONE
adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])) >> 2; //a pitch
adc_value[5] = -((rawADC_BMA180[1]<<8) | (rawADC_BMA180[0])) >> 2; //a roll
adc_value[6] = ((rawADC_BMA180[5]<<8) | (rawADC_BMA180[4])) >> 2; //a yaw
#endif
#ifdef FREEIMU_35
adc_value[4] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])) >> 2; //a pitch
adc_value[5] = -((rawADC_BMA180[1]<<8) | (rawADC_BMA180[0])) >> 2; //a roll
adc_value[6] = ((rawADC_BMA180[5]<<8) | (rawADC_BMA180[4])) >> 2; //a yaw
#endif
#ifdef FFIMU
adc_value[5] = ((rawADC_BMA180[3]<<8) | (rawADC_BMA180[2])) >> 2; //a pitch
adc_value[4] = ((rawADC_BMA180[1]<<8) | (rawADC_BMA180[0])) >> 2; //a roll
adc_value[6] = ((rawADC_BMA180[5]<<8) | (rawADC_BMA180[4])) >> 2; //a yaw
#endif
last_accel_read_millis = millis();
}
}
// Read one channel value, actually it uses to read only Sonar data (at 50Hz)
int AP_ADC_ADS7844::Ch(unsigned char ch_num)
{char i;int flt;
if ( (sonar_data < 354) && (pre_sonar_data > 0) ) { //wrong data from sonar (3cm * 118 = 354), use previous value
sonar_data=pre_sonar_data;
} else {
pre_sonar_data=sonar_data;
}
return(sonar_data / 118); // Magic conversion sonar_data to cm
}
// Read 6 channel values
uint32_t AP_ADC_ADS7844::Ch6(const uint8_t *channel_numbers, int *result)
{
i2c_ACC_getADC (); // Read sensors gyros each 4ms (because DCM called this method from fast_loop which run at 250Hz)
for (uint8_t i=0; i<6; i++) {
result[i] = adc_value[channel_numbers[i]];
}
// return number of microseconds since last call
uint32_t us = micros();
uint32_t ret = us - last_ch6_micros;
last_ch6_micros = us;
return ret;
}
codice:
/*
APM_RC.cpp - Radio Control Library for ArduPirates Arduino Mega with IPWM
Total rewritten by Syberian
Methods:
Init() : Initialization of interrupts an Timers
OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
InputCh(ch) : Read a channel input value. ch=0..7
GetState() : Returns the state of the input. 1 => New radio frame to process
Automatically resets when we call InputCh to read channels
*/
/*
APM motor remap to the MultiWii-style
Another remap to foolish the original board:
AP: 0,1,2,3,6,7 - motors, 4,5 - camstab (who the f*ck has implemented this???)
mw: 0,1,3,4,5,6 - motors
*/
#include "APM_RC.h"
#include <avr/interrupt.h>
#include "WProgram.h"
//######################################################
// ENABLE serial PPM receiver by uncommenting the line below
// Connect PPM_SUM signal to A8
//#define SERIAL_SUM
//##################################################
// Define one from your Receiver/Serial channels set. This can be used for both the PPM SUM and the normal point-to-point receiver to arduino connections
//#define TX_set1 //Graupner/Spektrum PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,CAMPITCH,CAMROLL
//#define TX_standard //standard PPM layout Robbe/Hitec/Sanwa ROLL,PITCH,THROTTLE,YAW,MODE,AUX2,CAMPITCH,CAMROLL
//#define TX_standard_mode6 //standard, Mode channel is 6 PPM layout Robbe/Hitec/Sanwa ROLL,PITCH,THROTTLE,YAW,AUX1,MODE,CAMPITCH,CAMROLL
//#define TX_set2 // some Hitec/Sanwa/others PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
#define TX_mwi // MultiWii layout ROLL,THROTTLE,PITCH,YAW,AUX1,AUX2,CAMPITCH,CAMROLL
//##################################################
#if (!defined(__AVR_ATmega1280__))&&(!defined(__AVR_ATmega2560__))
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
#else
// Variable definition for Input Capture interrupt
volatile uint8_t radio_status=0;
// ******************
// rc functions split channels
// ******************
volatile uint16_t rcPinValue[8] = {900,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]
// Configure each rc pin for PCINT
void configureReceiver() {
// PCINT activated only for specific pin inside [A8-A15]
DDRK = 0; // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)
PORTK = (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTK
#ifdef SERIAL_SUM
PCMSK2=1; // Enable int for pin A8
#else
PCMSK2 = 255; //
#endif
PCMSK0 = B00010000; // sonar port B4 - d10 echo
PCICR = B101; // PCINT activated only for PORTK dealing with [A8-A15] PINs
}
ISR(PCINT2_vect) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a digital pin [D2-D7]
static uint8_t mask;
static uint8_t pin;
static uint16_t cTime,dTime;
static uint16_t edgeTime[8];
static uint8_t PCintLast;
cTime = TCNT5; // from sonar
pin = PINK; // PINK indicates the state of each PIN for the arduino port dealing with [A8-A15] digital pins (8 bits variable)
mask = pin ^ PCintLast; // doing a ^ between the current interruption and the last one indicates wich pin changed
sei(); // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely
PCintLast = pin; // we memorize the current state of all PINs [D0-D7]
#ifdef SERIAL_SUM
static uint8_t pps_num=0;
static uint16_t pps_etime=0;
if (pin & 1) { // Rising edge detection
if (cTime < pps_etime) // Timer overflow detection
dTime = (0xFFFF-pps_etime)+cTime;
else
dTime = cTime-pps_etime;
if (dTime < 4400) {
rcPinValue[pps_num] = dTime>>1;
pps_num++;
pps_num&=7; // upto 8 packets in slot
} else
pps_num=0;
pps_etime = cTime; // Save edge time
}
#else // generic split PPM
// mask is pins [D0-D7] that have changed // the principle is the same on the MEGA for PORTK and [A8-A15] PINs
// chan = pin sequence of the port. chan begins at D2 and ends at D7
if (mask & 1<<0)
if (!(pin & 1<<0)) {
dTime = cTime-edgeTime[0]; if (1600<dTime && dTime<4400) rcPinValue[0] = dTime>>1;
} else edgeTime[0] = cTime;
if (mask & 1<<1)
if (!(pin & 1<<1)) {
dTime = cTime-edgeTime[1]; if (1600<dTime && dTime<4400) rcPinValue[1] = dTime>>1;
} else edgeTime[1] = cTime;
if (mask & 1<<2)
if (!(pin & 1<<2)) {
dTime = cTime-edgeTime[2]; if (1600<dTime && dTime<4400) rcPinValue[2] = dTime>>1;
} else edgeTime[2] = cTime;
if (mask & 1<<3)
if (!(pin & 1<<3)) {
dTime = cTime-edgeTime[3]; if (1600<dTime && dTime<4400) rcPinValue[3] = dTime>>1;
} else edgeTime[3] = cTime;
if (mask & 1<<4)
if (!(pin & 1<<4)) {
dTime = cTime-edgeTime[4]; if (1600<dTime && dTime<4400) rcPinValue[4] = dTime>>1;
} else edgeTime[4] = cTime;
if (mask & 1<<5)
if (!(pin & 1<<5)) {
dTime = cTime-edgeTime[5]; if (1600<dTime && dTime<4400) rcPinValue[5] = dTime>>1;
} else edgeTime[5] = cTime;
if (mask & 1<<6)
if (!(pin & 1<<6)) {
dTime = cTime-edgeTime[6]; if (1600<dTime && dTime<4400) rcPinValue[6] = dTime>>1;
} else edgeTime[6] = cTime;
if (mask & 1<<7)
if (!(pin & 1<<7)) {
dTime = cTime-edgeTime[7]; if (1600<dTime && dTime<4400) rcPinValue[7] = dTime>>1;
} else edgeTime[7] = cTime;
#endif
}
/* RC standard matrix (we are using analog inputs A8..A15 of MEGA board)
0 Aileron
1 Elevator
2 Throttle
3 Rudder
4 RX CH 5 Gear
5 RX CH 6 Flaps
5.bis RX CH 6 Flaps 3 Switch
6 RX CH7
7 *) RX CH8
*/
//MultiWii compatibility layout:
/*
THROTTLEPIN //PIN 62 = PIN A8
ROLLPIN //PIN 63 = PIN A9
PITCHPIN //PIN 64 = PIN A10
YAWPIN //PIN 65 = PIN A11
AUX1PIN //PIN 66 = PIN A12
AUX2PIN //PIN 67 = PIN A13
CAM1PIN //PIN 68 = PIN A14
CAM2PIN //PIN 69 = PIN A15
*/
#ifdef TX_set1
static uint8_t pinRcChannel[8] = {1, 3, 2, 0, 4,5,6,7}; //Graupner/Spektrum
#endif
#ifdef TX_standard
static uint8_t pinRcChannel[8] = {0, 1, 2, 3, 4,5,6,7}; //standard PPM layout Robbe/Hitec/Sanwa
#endif
#ifdef TX_standard_mode6
static uint8_t pinRcChannel[8] = {0, 1, 2, 3, 5,4,6,7}; //standard layout with swapped 5,6 channels (Mode switch on 6 channel)
#endif
#ifdef TX_set2
static uint8_t pinRcChannel[8] = {1, 0, 2, 3, 4,5,6,7}; // some Hitec/Sanwa/others
#endif
#ifdef TX_mwi
static uint8_t pinRcChannel[8] = {1, 2, 0, 3, 4,5,6,7}; // mapped multiwii to APM layout
#endif
uint16_t readRawRC(uint8_t chan) {
uint16_t data;
uint8_t oldSREG;
oldSREG = SREG;
cli(); // Let's disable interrupts
data = rcPinValue[pinRcChannel[chan]]; // Let's copy the data Atomically
SREG = oldSREG;
sei();// Let's enable the interrupts
return data; // We return the value correctly copied when the IRQ's where disabled
}
//######################### END RC split channels
// Constructors ////////////////////////////////////////////////////////////////
/*
timer usage:
0 8bit
1 general servo
2 8bit
3 servo3,5,2 (OCR ABC)
4 servo6,7,8
5 rc input and sonar
*/
// Constructors ////////////////////////////////////////////////////////////////
APM_RC_Class::APM_RC_Class()
{
}
// Public Methods //////////////////////////////////////////////////////////////
void APM_RC_Class::Init(void)
{
//We are using JUST 1 timer1 for 16 PPM outputs!!! (Syberian)
pinMode(2,OUTPUT);
pinMode(3,OUTPUT);
pinMode(4,OUTPUT);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(7,OUTPUT);
pinMode(8,OUTPUT);
pinMode(9,OUTPUT);
pinMode(11,OUTPUT);
pinMode(12,OUTPUT);
pinMode(44,OUTPUT);//cam roll L5
pinMode(45,OUTPUT);// cam pitch L4
//general servo
TCCR5A =0; //standard mode with overflow at A and OC B and C interrupts
TCCR5B = (1<<CS11); //Prescaler set to 8, resolution of 0.5us
TIMSK5=B00000111; // ints: overflow, capture, compareA
OCR5A=65510; // approx 10m limit, 33ms period
OCR5B=3000;
//motors
OCR1A = 1800;
OCR1B = 1800;
ICR1 = 40000; //50hz freq...Datasheet says (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000,
TCCR1A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)); // A and B used
TCCR1B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR3A = 1800;
OCR3B = 1800;
OCR3C = 1800;
ICR3 = 40000; //50hz freq
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR4A = 1800;
OCR4B = 1800;
OCR4C = 1800;
ICR4 = 40000; //50hz freq
TCCR4A =((1<<WGM31)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1));
TCCR4B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
configureReceiver();
}
uint16_t OCRxx1[8]={1800,1800,1800,1800,1800,1800,1800,1800,};
char OCRstate=7;
/*
D Port PWM
2 e4 0 3B
3 e5 1 3C
4 g5 2
5 e3 3 3A
6 h3 4 4A
7 h4 5 4B
8 h5 6 5C
9 h6 7
//2nd gro6up
22 a0 8
23 a1 9
24 a2 10
25 a3 11
26 a4 12
27 a5 13
28 a6 14
29 a7 15
*/
ISR(TIMER5_COMPB_vect)
{ // set the corresponding pin to 1
OCRstate++;
OCRstate&=15;
switch (OCRstate>>1)
{//case 0: if(OCRstate&1)PORTC&=(1<<5)^255; else PORTC|=(1<<5);break; //d32, cam roll
//case 1: if(OCRstate&1)PORTC&=(1<<4)^255; else PORTC|=(1<<4);break; //d33, cam pitch
case 0: if(OCRstate&1)PORTL&=(1<<5)^255; else PORTL|=(1<<5);break; //d44, cam Roll
case 1: if(OCRstate&1)PORTL&=(1<<4)^255; else PORTL|=(1<<4);break; //d45, cam Pitch
//case 2: PORTG|=(1<<5);break;
//case 3: PORTE|=(1<<3);break;
//case 4: PORTH|=(1<<3);break;
//case 5: PORTH|=(1<<4);break;
//case 6: PORTH|=(1<<5);break;
//case 7: PORTH|=(1<<6);break;
}
if(OCRstate&1)OCR5B+=5000-OCRxx1[OCRstate>>1]; else OCR5B+=OCRxx1[OCRstate>>1];
}
/*
ch 3 4 1 2 7 8 10 11
motor mapping
=======================================================================
Pin 2 3 5 6 7 8 11 12
=======================================================================
TRI S BC RC LC - - - -
QuadX LFW RBW RFC LBC - - - -
QuadP FW BW RC LC - - - -
HexaP BLW FRC FW BC FLC BRW - -
HexaX FLW BRC RW LC FRC BLW - -
Y6 LDW BDW RDW LUC RUC BUC - -
OCTA_X
OCTA_P
=============
Motors description:
B- back
R- right
L- left
F- front
U- upper
D- lower
W- clockwise rotation
C- counter clockwise rotation (normal propeller)
S- servo (for tri)
Example: FLDW - front-left lower motor with clockwise rotation (Y6 or Y4)
*/
void APM_RC_Class::OutputCh(uint8_t ch, uint16_t pwm)
{
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
pwm<<=1; // pwm*2;
switch(ch)
{
case 0: OCR3A=pwm; break; //5
case 1: OCR4A=pwm; break; //6
case 2: OCR3B=pwm; break; //2
case 3: OCR3C=pwm; break; //3
case 4: OCRxx1[1]=pwm;break;//OCRxx1[ch]=pwm;CAM PITCH
case 5: OCRxx1[0]=pwm;break;//OCRxx1[ch]=pwm;CAM ROLL
case 6: OCR4B=pwm; break; //7
case 7: OCR4C=pwm; break; //8
case 9: OCR1A=pwm;break;// d11
case 10: OCR1B=pwm;break;// d12
case 8: //2nd group
case 11:
case 12:
case 13:
case 14:
case 15: break;//OCRxx2[ch-8]=(tempx*5)/79;break;
}
}
uint16_t APM_RC_Class::InputCh(uint8_t ch)
{
uint16_t result;
uint16_t result2;
// Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
// We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
result=readRawRC(ch);
// Limit values to a valid range
result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
radio_status=1; // Radio channel read
return(result);
}
uint8_t APM_RC_Class::GetState(void)
{
return(1);// always 1
}
// InstantPWM implementation
// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use
void APM_RC_Class::Force_Out0_Out1(void)
{
if (TCNT3>5000) // We take care that there are not a pulse in the output
TCNT3=39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000
if (TCNT4>5000)
TCNT4=39990;
if (TCNT1>5000)
TCNT1=39990;
}
// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use
void APM_RC_Class::Force_Out2_Out3(void)
{
// if (TCNT1>5000)
// TCNT1=39990;
}
// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use
void APM_RC_Class::Force_Out6_Out7(void)
{
// if (TCNT3>5000)
// TCNT3=39990;
}
// allow HIL override of RC values
// A value of -1 means no change
// A value of 0 means no override, use the real RC values
bool APM_RC_Class::setHIL(int16_t v[NUM_CHANNELS])
{
/*
uint8_t sum = 0;
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
if (v[i] != -1) {
_HIL_override[i] = v[i];
}
if (_HIL_override[i] != 0) {
sum++;
}
}
radio_status = 1;
if (sum == 0) {
return 0;
} else {
return 1;
}
*/
radio_status = 1;
return 1;
}
void APM_RC_Class::clearOverride(void)
{
for (uint8_t i=0; i<NUM_CHANNELS; i++) {
_HIL_override[i] = 0;
}
}
// make one instance for the user to use
APM_RC_Class APM_RC;
#endif // defined(ATMega1280)
-Upload con ARDUINO 0022 (ok)
3°
-Collegato la Flyduino con FTDI-USB
-Collegato la BATTERIA LIPO
-Ponticellato il PIN dedicato CLI a GND per entrare in setup mode
-Connect (atteso qualche secondo)
-Terminale
4°
-Da terminale ho eseguito SETUP
-RADIO ed eseguito la calibrazione stick
-LEVEL ed eseguito la calibrazione degli accellerometri
-COMPASS ma dà solo 2 opzioni [ON,OFF] Quindi COMPASS ON
Compass
----------------------------------------
enabled
-ESC devo ancora farla
-SHOW ma vedo che il MAG non è calibrato e poi credo di avere anche un altro problema (140:IMU_SENSOR_CAL : ?? )
codice:
FW Version 111 ---------------------------------------- Radio ---------------------------------------- CH1: 995 | 2000 | trim 1523 CH2: 996 | 2013 | trim 1523 CH3: 985 | 2016 CH4: 998 | 2007 | trim 1525 CH5: 997 | 2006 CH6: 986 | 2004 CH7: 1500 | 1500 Frame ---------------------------------------- Hexa frame X mode Batt Mointor ---------------------------------------- disabled Sonar ---------------------------------------- disabled Flight modes ---------------------------------------- Pos 0: ACRO, Simple: OFF Pos 1: STABILIZE, Simple: OFF Pos 2: STABILIZE, Simple: OFF Pos 3: STABILIZE, Simple: OFF Pos 4: STABILIZE, Simple: OFF Pos 5: LOITER, Simple: OFF IMU ---------------------------------------- Gyro offsets: 33.00, 49.11, 38.81 Accel offsets: -175.56, 244.87, 80.04 Compass ---------------------------------------- enabled Mag Dec: -1.3245 Mag offsets: 0.0000, 0.0000, 0.0000 140:IMU_SENSOR_CAL : ??Quì penso ci sia un problema... 199:CAM_R_G : 1.000000 198:CAM_P_G : 1.000000 150:LOW_VOLT : 9.600000 149:IN_VOLT : 5.000000 148:FLOW_ENABLE : 0 147:TB_RATIO : 0.920000 146:FRAME : 1 197:TUNE : 0 196:ESC : 0 143:MAG_ENABLE : 1 142:BATT_CAPACITY : 1760 141:BATT_MONITOR : 0 145:SONAR_ENABLE : 0 161:ALT_HOLD_RTL : 1000 002:LOG_BITMASK : 380 206:SIMPLE : 0 205:FLTMODE6 : 5 204:FLTMODE5 : 0 203:FLTMODE4 : 0 202:FLTMODE3 : 0 201:FLTMODE2 : 0 200:FLTMODE1 : 1 195:TRIM_THROTTLE : 447 194:THR_FS_VALUE : 975 193:THR_FS_ACTION : 2 192:THR_FAILSAFE : 0 191:THR_MAX : 1000 190:THR_MIN : 0 226:WP_SPEED_MAX : 400 225:WP_LOITER_RAD : -24 224:WP_RADIUS : 3 223:WP_MUST_INDEX : 0 222:WP_INDEX : 0 221:WP_TOTAL : 0 220:WP_MODE : 0 160:XTRK_ANGLE_CD : 3000 114:SERIAL3_BAUD : 57 113:SYSID_MYGCS : 255 112:SYSID_THISMAV : 1 001:SYSID_SW_TYPE : 10 000:SYSID_SW_MREV : 111 65535:SR3_RAW_SENS : 0 65535:SR0_RAW_SENS : 0 65535:XTRACK_P : 2.000000 65535:THR_RATE_P : 0.599609 65535:THR_ALT_P : 0.500000 65535:NAV_LON_P : 3.000000 65535:NAV_LAT_P : 3.000000 65535:HLD_LON_P : 0.299805 65535:HLD_LAT_P : 0.299805 65535:STB_YAW_P : 7.000000 65535:STB_PIT_P : 4.599609 65535:STB_RLL_P : 4.599609 65535:RATE_YAW_P : 0.129883 65535:RATE_PIT_P : 0.144531 65535:RATE_RLL_P : 0.144531 65535:CAM_R_MIN : 1100 65535:CAM_P_MIN : 1100 65535:RC8_MIN : 1500 65535:RC7_MIN : 1500 65535:RC6_MIN : 986 65535:RC5_MIN : 997 65535:RC4_MIN : 998 65535:RC3_MIN : 985 65535:RC2_MIN : 996 65535:RC1_MIN : 995 65535:SR3_EXT_STAT : 0 65535:SR0_EXT_STAT : 0 65535:XTRACK_I : 0.000000 65535:THR_RATE_I : 0.099609 65535:THR_ALT_I : 0.009766 65535:NAV_LON_I : 0.250000 65535:NAV_LAT_I : 0.250000 65535:HLD_LON_I : 0.000000 65535:HLD_LAT_I : 0.000000 65535:STB_YAW_I : 0.009766 65535:STB_PIT_I : 0.000977 65535:STB_RLL_I : 0.000977 65535:RATE_YAW_I : 0.000000 65535:RATE_PIT_I : 0.000000 65535:RATE_RLL_I : 0.000000 65535:CAM_R_TRIM : 1500 65535:CAM_P_TRIM : 1500 65535:RC8_TRIM : 1500 65535:RC7_TRIM : 1500 65535:RC6_TRIM : 1500 65535:RC5_TRIM : 1500 65535:RC4_TRIM : 1525 65535:RC3_TRIM : 1500 65535:RC2_TRIM : 1523 65535:RC1_TRIM : 1523 65535:SR3_RC_CHAN : 0 65535:SR0_RC_CHAN : 0 65535:COMPASS_DEC : -0.023117 65535:CAM_R_MAX : 1900 65535:CAM_P_MAX : 1900 65535:RC8_MAX : 1500 65535:RC7_MAX : 1500 65535:RC6_MAX : 2004 65535:RC5_MAX : 2006 65535:RC4_MAX : 2007 65535:RC3_MAX : 2016 65535:RC2_MAX : 2013 65535:RC1_MAX : 2000 65535:SR3_RAW_CTRL : 0 65535:SR0_RAW_CTRL : 0 65535:XTRACK_IMAX : 10 65535:THR_RATE_IMAX : 50 65535:THR_ALT_IMAX : 300 65535:NAV_LON_IMAX : 2000 65535:NAV_LAT_IMAX : 2000 65535:HLD_LON_IMAX : 1200 65535:HLD_LAT_IMAX : 1200 65535:STB_YAW_IMAX : 800 65535:STB_PIT_IMAX : 150 65535:STB_RLL_IMAX : 150 65535:RATE_YAW_IMAX : 5000 65535:RATE_PIT_IMAX : 1500 65535:RATE_RLL_IMAX : 1500 65535:CAM_R_REV : 1 65535:CAM_P_REV : 1 65535:RC8_REV : 1 65535:RC7_REV : 1 65535:RC6_REV : 1 65535:RC5_REV : 1 65535:RC4_REV : 1 65535:RC3_REV : 1 65535:RC2_REV : 1 65535:RC1_REV : 1 65535:SR3_POSITION : 0 65535:SR0_POSITION : 0 65535:SR3_EXTRA1 : 0 65535:SR0_EXTRA1 : 0 65535:SR3_EXTRA2 : 0 65535:SR0_EXTRA2 : 0 65535:SR3_EXTRA3 : 0 65535:SR0_EXTRA3 : 0
Commenta