Hi liebe Gemeinde,<br>
ich verzweifel so langsam an meinem Gps für mein Multiwii! Ich habe mir gedacht, wer mir Tips, oder den entscheiden Tip gibt mein Gps zum laufen zu bekommen, der wird mit ner Kiste Bier nach Wahl, Kiste Cola oder sonstwas entlohnt, das ist versprochen !<br>
Hier zuerst meine Hardware:<br>
<br>
<a href="http://www.ebay.com.au/itm/MWC-MultiWii-SE-Multicopter-control-board-w-USB-PRGMR-GPS-NAV-Module-Receiver-/221075867343?pt=Radio_Control_Vehicles&hash=item33792606cf" target="_blank" rel="nofollow">http://www.ebay.com.au/itm/MWC-MultiWii-SE-Multicopter-control-board-w-USB-PRGMR-GPS-NAV-Module-Receiver-/221075867343?pt=Radio_Control_Vehicles&hash=item33792606cf</a><br>
<br>
Das Multiwii fliegt in meinem F450 perfekt!<br>
<br>
Das Problem ist halt nur das GPS.<br>
<br>
Was habe ich bis jetzt gemacht:<br>
NavBoard mit I2C_GPS_NAV-Multiwii2.0-GPS_only geflashed --- nutze MultWii 2.1, denke aber nicht das die Nachfolgeversion probleme bereitet, eher die Vorgängerversion!?- hier der Sketch:<br>
<br>
<br>
<br>
GPS wird in der GUI grün angezeigt. Ich habe I2C Errors und finde keine Sateliten.<br>
Das GPS Modul leuchted dauergrün, das NavBoard blinkt smoth im Sekundenanstand.<br>
Muss der GPS Empfänger selber auch geflashed werden ? Ich habe noch nirgendswo gefunden, wo ich im Arduino beim NavBoard zwischen der Hz Zahlen wählen kann (5 oder 10Hz)<br>
<br>
Wer weitere Infos braucht, einfach melden, gerne können wir auch telefonieren, wenn du in der Umgebung von Gütersloh/ Bielefeld wohnst komme ich auch gerne mit dem ganzen Krempel vorbei.<br>
Gruß<br>
Mirco
ich verzweifel so langsam an meinem Gps für mein Multiwii! Ich habe mir gedacht, wer mir Tips, oder den entscheiden Tip gibt mein Gps zum laufen zu bekommen, der wird mit ner Kiste Bier nach Wahl, Kiste Cola oder sonstwas entlohnt, das ist versprochen !<br>
Hier zuerst meine Hardware:<br>
<br>
<a href="http://www.ebay.com.au/itm/MWC-MultiWii-SE-Multicopter-control-board-w-USB-PRGMR-GPS-NAV-Module-Receiver-/221075867343?pt=Radio_Control_Vehicles&hash=item33792606cf" target="_blank" rel="nofollow">http://www.ebay.com.au/itm/MWC-MultiWii-SE-Multicopter-control-board-w-USB-PRGMR-GPS-NAV-Module-Receiver-/221075867343?pt=Radio_Control_Vehicles&hash=item33792606cf</a><br>
<br>
Das Multiwii fliegt in meinem F450 perfekt!<br>
<br>
Das Problem ist halt nur das GPS.<br>
<br>
Was habe ich bis jetzt gemacht:<br>
NavBoard mit I2C_GPS_NAV-Multiwii2.0-GPS_only geflashed --- nutze MultWii 2.1, denke aber nicht das die Nachfolgeversion probleme bereitet, eher die Vorgängerversion!?- hier der Sketch:<br>
<br>
Code:
<br>
(I2C_GPS_NAV)<br>
<br>
/*******************************************************************************************************************************<br>
* I2CGPS - Inteligent GPS and NAV module for MultiWii by EOSBandi<br>
* V2.1 <br>
*<br>
* This program implements position hold and navigational functions for MultiWii by offloading caclulations and gps parsing <br>
* into a secondary arduino processor connected to the MultiWii via i2c.<br>
* Once activated it outputs desired banking in a lat/lon coordinate system, which can be easily rotated into the copter's frame of reference.<br>
*<br>
* Navigation and Position hold routines and PI/PID libraries are based on code and ideas from the Arducopter team:<br>
* Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen<br>
* Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni<br>
* Status blink code from Guru_florida<br>
* <br>
* This program is free software: you can redistribute it and/or modify<br>
* it under the terms of the GNU General Public License as published by<br>
* the Free Software Foundation, either version 3 of the License, or<br>
* any later version. see <http://www.gnu.org/licenses/><br>
* This library is distributed in the hope that it will be useful,<br>
* but WITHOUT ANY WARRANTY; without even the implied warranty of<br>
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU<br>
* General Public License for more details.<br>
***********************************************************************************************************************************/<br>
<br>
#define VERSION 21 //Software version for cross checking<br>
<br>
<br>
#include "WireMW.h"<br>
#include "PIDCtrl.h"<br>
#include "PICtrl.h"<br>
#include "LeadFilter.h"<br>
<br>
<br>
#include "registers.h" //Register definitions<br>
#include "config.h"<br>
<br>
#define REG_MAP_SIZE sizeof(i2c_dataset) //size of register map<br>
#define MAX_SENT_BYTES 0x0C //maximum amount of data that I could receive from a master device (register, plus 11 byte waypoint data)<br>
<br>
#define LAT 0<br>
#define LON 1<br>
<br>
///////////////////////////////////////////////////////////////////////////////////////////////////<br>
// Which command I got from the host ? in GPSMode variable<br>
#define GPSMODE_NONAV 0 // no navigation<br>
#define GPSMODE_HOLD 1 // pos hold<br>
#define GPSMODE_WP 2 // wp navigation<br>
///////////////////////////////////////////////////////////////////////////////////////////////////<br>
// Navigation state machine states (nav_mode)<br>
#define NAV_MODE_NONE 0<br>
#define NAV_MODE_POSHOLD 1<br>
#define NAV_MODE_WP 2<br>
// Convert deg*100 to radians<br>
#define RADX100 0.000174532925 <br>
//Blink feedback, by guru_florida<br>
#define BLINK_INTERVAL 90<br>
<br>
typedef struct {<br>
uint8_t new_data:1;<br>
uint8_t gps2dfix:1;<br>
uint8_t gps3dfix:1;<br>
uint8_t wp_reached:1;<br>
uint8_t numsats:4;<br>
} STATUS_REGISTER;<br>
<br>
typedef struct {<br>
uint8_t command:4;<br>
uint8_t wp:4;<br>
} COMMAND_REGISTER;<br>
<br>
<br>
typedef struct {<br>
uint8_t active_wp:4;<br>
uint8_t pervious_wp:4;<br>
} WAYPOINT_REGISTER;<br>
<br>
typedef struct {<br>
long lat; //degree*10 000 000<br>
long lon; //degree*10 000 000<br>
} GPS_COORDINATES;<br>
<br>
typedef struct {<br>
GPS_COORDINATES position; //GPS coordinate of the waypoint<br>
uint16_t altitude; //altitude of the waypoint in meters (altitude is relative to the startup altitude)<br>
uint8_t flags; //to be defined<br>
} WAYPOINT;<br>
<br>
typedef struct {<br>
<br>
//Status and command registers<br>
STATUS_REGISTER status; // 0x00 status register<br>
COMMAND_REGISTER command; // 0x01 command register<br>
WAYPOINT_REGISTER wp_register; // 0x02 waypoint register (current, previus)<br>
uint8_t sw_version; // 0x03 Version of the I2C_GPS sw<br>
uint8_t res3; // 0x04 reserved for future use<br>
uint8_t res4; // 0x05 reserved for future use<br>
uint8_t res5; // 0x06 reserved for future use<br>
<br>
//GPS & navigation data<br>
GPS_COORDINATES gps_loc; // current location (8 byte) lat,lon<br>
int16_t nav_lat; // The desired bank towards North (Positive) or South (Negative) 1 deg = 100 max 30deg (3000)<br>
int16_t nav_lon; // The desired bank towards East (Positive) or West (Negative) 1 deg = 100 max 30deg (3000)<br>
uint32_t wp_distance; // distance to active coordinates (calculated) in cm<br>
int16_t wp_target_bearing; // direction to active coordinates (calculated) 1deg = 10 / -1800 - 1800<br>
int16_t nav_bearing; // crosstrack corrected navigational bearing 1deg = 10<br>
int16_t home_to_copter_bearing; // 1deg = 10<br>
uint16_t distance_to_home; // distance to home in cm<br>
uint16_t ground_speed; // ground speed from gps m/s*100<br>
int16_t altitude; // gps altitude<br>
uint16_t ground_course; // GPS ground course<br>
uint16_t res6; // reserved for future use<br>
uint32_t time; // UTC Time from GPS<br>
<br>
//Parameters<br>
uint8_t nav_crosstrack_gain; // Crosstrack gain *100 (1 - 0.01 , 100 - 1)<br>
uint8_t nav_speed_min; // minimum speed for navigation cm/s<br>
uint16_t nav_speed_max; // maxiumum speed for navigation cm/s<br>
uint16_t nav_bank_max; // maximum banking 1deg = 100, 30deg = 3000<br>
uint16_t wp_radius; // waypoint radius, if we within this radius, then we considered that the wp is reached in cm<br>
uint8_t nav_flags; // navigational flags to be defined<br>
<br>
<br>
//PID values<br>
uint8_t poshold_p; // *100<br>
uint8_t poshold_i; // *100<br>
uint8_t poshold_imax; // *1<br>
<br>
uint8_t poshold_rate_p; // *10<br>
uint8_t poshold_rate_i; // *100<br>
uint8_t poshold_rate_d; // *1000<br>
uint8_t poshold_rate_imax; // *1<br>
<br>
uint8_t nav_p; // *10<br>
uint8_t nav_i; // *100<br>
uint8_t nav_d; // *1000<br>
uint8_t nav_imax; // *1<br>
<br>
WAYPOINT gps_wp[16]; // 16 waypoints, WP#0 is RTH position<br>
} I2C_REGISTERS;<br>
<br>
static I2C_REGISTERS i2c_dataset;<br>
<br>
static GPS_COORDINATES _target; //internal target location register<br>
<br>
static uint8_t receivedCommands[MAX_SENT_BYTES];<br>
static uint8_t new_command; //new command received (!=0)<br>
<br>
<br>
//////////////////////////////////////////////////////////////////////////////<br>
// Variables and controllers<br>
<br>
//PID controllers<br>
<br>
PICtrl pi_poshold_lat(POSHOLD_P, POSHOLD_I, POSHOLD_IMAX * 100);<br>
PICtrl pi_poshold_lon(POSHOLD_P, POSHOLD_I, POSHOLD_IMAX * 100);<br>
PIDCtrl pid_poshold_rate_lat(POSHOLD_RATE_P, POSHOLD_RATE_I, POSHOLD_RATE_D, POSHOLD_RATE_IMAX * 100);<br>
PIDCtrl pid_poshold_rate_lon(POSHOLD_RATE_P, POSHOLD_RATE_I, POSHOLD_RATE_D, POSHOLD_RATE_IMAX * 100);<br>
PIDCtrl pid_nav_lat(NAV_P,NAV_I,NAV_D,NAV_IMAX * 100);<br>
PIDCtrl pid_nav_lon(NAV_P,NAV_I,NAV_D,NAV_IMAX * 100);<br>
<br>
LeadFilter xLeadFilter; // Long GPS lag filter <br>
LeadFilter yLeadFilter; // Lat GPS lag filter <br>
<br>
// used to track the elapsed time between GPS reads<br>
static uint32_t nav_loopTimer;<br>
// Delta Time in milliseconds for navigation computations, updated with every good GPS read<br>
static float dTnav;<br>
<br>
//Actual navigation mode, this needed since we swith to poshold ence arrived at home<br>
static int8_t nav_mode = NAV_MODE_NONE; //Navigation mode<br>
<br>
static int16_t x_actual_speed = 0;<br>
static int16_t y_actual_speed = 0;<br>
static int32_t last_longitude = 0;<br>
static int32_t last_latitude = 0;<br>
<br>
static int16_t x_rate_d;<br>
static int16_t y_rate_d;<br>
<br>
// this is used to offset the shrinking longitude as we go towards the poles<br>
static float GPS_scaleLonDown;<br>
static float GPS_scaleLonUp;<br>
<br>
// The difference between the desired rate of travel and the actual rate of travel<br>
// updated after GPS read - 5-10hz<br>
static int16_t x_rate_error;<br>
static int16_t y_rate_error;<br>
static int32_t long_error, lat_error;<br>
<br>
// The desired bank towards North (Positive) or South (Negative)<br>
static int16_t nav_lat;<br>
// The desired bank towards East (Positive) or West (Negative)<br>
static int16_t nav_lon;<br>
<br>
//Used for rotation calculations for GPS nav vector<br>
static float sin_yaw_y;<br>
static float cos_yaw_x;<br>
<br>
//Currently used WP<br>
static int32_t GPS_WP_latitude,GPS_WP_longitude;<br>
static uint16_t GPS_WP_altitude;<br>
static uint8_t GPS_WP_flags;<br>
<br>
//Actual position for calculations<br>
static int32_t GPS_latitude,GPS_longitude;<br>
<br>
////////////////////////////////////////////////////////////////////////////////<br>
// Location & Navigation<br>
////////////////////////////////////////////////////////////////////////////////<br>
// This is the angle from the copter to the "next_WP" location in degrees * 100<br>
static int32_t target_bearing;<br>
// This is the angle from the copter to the "next_WP" location<br>
// with the addition of Crosstrack error in degrees * 100<br>
static int32_t nav_bearing;<br>
// saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction when arrives at home<br>
static int16_t nav_takeoff_bearing; <br>
<br>
////////////////////////////////////////////////////////////////////////////////<br>
// Crosstrack<br>
////////////////////////////////////////////////////////////////////////////////<br>
// deg * 100, The original angle to the next_WP when the next_WP was set<br>
// Also used to check when we pass a WP<br>
static int32_t original_target_bearing;<br>
// The amount of angle correction applied to target_bearing to bring the copter back on its optimum path<br>
static int16_t crosstrack_error;<br>
////////////////////////////////////////////////////////////////////////////////<br>
// The location of the copter in relation to home, updated every GPS read (1deg - 100)<br>
static int32_t home_to_copter_bearing;<br>
// distance between plane and home in cm<br>
static int32_t home_distance;<br>
// distance between plane and next_WP in cm<br>
static int32_t wp_distance;<br>
<br>
// used for slow speed wind up when start navigation;<br>
static int16_t waypoint_speed_gov;<br>
<br>
// this is the navigation mode what is commanded<br>
static uint8_t GPSMode = GPSMODE_NONAV; <br>
<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Blink code variables<br>
//<br>
static uint32_t lastframe_time = 0;<br>
static uint32_t _statusled_timer = 0;<br>
static int8_t _statusled_blinks = 0;<br>
static boolean _statusled_state = 0;<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// moving average filter variables<br>
//<br>
static uint8_t GPS_filter_index = 0;<br>
static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];<br>
static int32_t GPS_filter_sum[2];<br>
static int32_t GPS_read[2];<br>
static int32_t GPS_filtered[2];<br>
static int32_t GPS_degree[2]; //the lat lon degree without any decimals (lat/10 000 000)<br>
static int32_t GPS_lead_latitude, GPS_lead_longitude;<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// this is used to offset the shrinking longitude as we go towards the poles <br>
// It's ok to calculate this once per waypoint setting, since it changes a little within the reach of a multicopter<br>
//<br>
void GPS_calc_longitude_scaling(int32_t lat)<br>
{<br>
float rads = (abs((float)lat) / 10000000.0) * 0.0174532925;<br>
GPS_scaleLonDown = cos(rads);<br>
GPS_scaleLonUp = 1.0f / cos(rads);<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values<br>
// Waypoint 16 is a virtual waypoint, it's a pos hold<br>
void GPS_set_next_wp(uint8_t wp_number)<br>
{<br>
if (wp_number > 16) { return; } //Do nothing with a wrong WP number<br>
<br>
if (wp_number <=15 ) {<br>
GPS_WP_latitude = i2c_dataset.gps_wp[wp_number].position.lat;<br>
GPS_WP_longitude = i2c_dataset.gps_wp[wp_number].position.lon;<br>
GPS_WP_altitude = i2c_dataset.gps_wp[wp_number].altitude;<br>
GPS_WP_flags = i2c_dataset.gps_wp[wp_number].flags;<br>
i2c_dataset.status.wp_reached = 0; <br>
} else {<br>
GPS_WP_latitude = GPS_latitude;<br>
GPS_WP_longitude = GPS_longitude;<br>
GPS_WP_altitude = 0;<br>
GPS_WP_flags = 0;<br>
i2c_dataset.status.wp_reached = 1; //With poshold we assume that the wp is reached <br>
} <br>
<br>
GPS_calc_longitude_scaling(GPS_WP_latitude);<br>
<br>
wp_distance = GPS_distance_cm(GPS_latitude,GPS_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
target_bearing = GPS_bearing(GPS_latitude,GPS_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
nav_bearing = target_bearing;<br>
GPS_calc_location_error(GPS_WP_latitude,GPS_WP_longitude,GPS_latitude,GPS_longitude);<br>
original_target_bearing = target_bearing;<br>
waypoint_speed_gov = i2c_dataset.nav_speed_min;<br>
<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Check if we missed the destination somehow<br>
//<br>
static bool check_missed_wp()<br>
{<br>
int32_t temp;<br>
temp = target_bearing - original_target_bearing;<br>
temp = wrap_18000(temp);<br>
return (abs(temp) > 10000); // we passed the waypoint by 100 degrees<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Get distance between two points in cm<br>
//<br>
uint32_t GPS_distance_cm(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2) {<br>
float dLat = (lat2 - lat1); // difference of latitude in 1/10 000 000 degrees<br>
float dLon = (float)(lon2 - lon1) * GPS_scaleLonDown;<br>
float dist = sqrt(sq(dLat) + sq(dLon)) * 1.113195;<br>
return dist;<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// get bearing from pos1 to pos2, returns an 1deg = 100 precision<br>
//<br>
int32_t GPS_bearing(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2)<br>
{<br>
float off_x = (float)lon2 - lon1;<br>
float off_y = ((float)(lat2 - lat1)) * GPS_scaleLonUp;<br>
float bearing = 9000.0f + atan2(-off_y, off_x) * 5729.57795f; //Convert the output redians to 100xdeg<br>
<br>
if (bearing < 0) bearing += 36000;<br>
return bearing;<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Calculate our current speed vector from gps position data<br>
//<br>
static void GPS_calc_velocity( int32_t gps_latitude, int32_t gps_longitude){<br>
<br>
static int16_t x_speed_old = 0;<br>
static int16_t y_speed_old = 0;<br>
<br>
// y_GPS_speed positve = Up<br>
// x_GPS_speed positve = Right<br>
<br>
// initialise last_longitude and last_latitude<br>
if( last_longitude == 0 && last_latitude == 0 ) {<br>
last_longitude = gps_longitude;<br>
last_latitude = gps_latitude;<br>
}<br>
<br>
float tmp = 1.0/dTnav;<br>
x_actual_speed = (float)(gps_longitude - last_longitude) * GPS_scaleLonDown * tmp;<br>
y_actual_speed = (float)(gps_latitude - last_latitude) * tmp;<br>
<br>
#if !defined(GPS_LEAD_FILTER)<br>
x_actual_speed = (x_actual_speed + x_speed_old) / 2;<br>
y_actual_speed = (y_actual_speed + y_speed_old) / 2;<br>
x_speed_old = x_actual_speed;<br>
y_speed_old = y_actual_speed;<br>
#endif<br>
<br>
last_longitude = gps_longitude;<br>
last_latitude = gps_latitude;<br>
<br>
#if defined(GPS_LEAD_FILTER)<br>
GPS_lead_longitude = xLeadFilter.get_position(gps_longitude,x_actual_speed);<br>
GPS_lead_latitude = yLeadFilter.get_position(gps_latitude,y_actual_speed);<br>
#endif<br>
<br>
<br>
}<br>
<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Calculate a location error between two gps coordinates<br>
// Becuase we are using lat and lon to do our distance errors here's a quick chart:<br>
// 100 = 1m<br>
// 1000 = 11m = 36 feet<br>
// 1800 = 19.80m = 60 feet<br>
// 3000 = 33m<br>
// 10000 = 111m<br>
//<br>
static void GPS_calc_location_error( int32_t target_lat, int32_t target_lng, int32_t gps_lat, int32_t gps_lng )<br>
{<br>
// X Error<br>
long_error = (float)(target_lng - gps_lng) * GPS_scaleLonDown; <br>
// Y Error<br>
lat_error = target_lat - gps_lat; <br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Calculate nav_lat and nav_lon from the x and y error and the speed<br>
//<br>
static void GPS_calc_poshold(int x_error, int y_error)<br>
{<br>
int32_t p,i; <br>
int32_t output;<br>
int32_t x_target_speed, y_target_speed;<br>
<br>
// East / West<br>
x_target_speed = pi_poshold_lon.get_p(x_error); // calculate desired speed from lon error<br>
x_target_speed = constrain(x_target_speed,-100,100);<br>
x_rate_error = x_target_speed - x_actual_speed; // calc the speed error<br>
<br>
p = pid_poshold_rate_lon.get_p(x_rate_error);<br>
i = pid_poshold_rate_lon.get_i(x_rate_error + x_error, dTnav);<br>
output = p + i;<br>
nav_lon = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX); <br>
<br>
// North / South<br>
y_target_speed = pi_poshold_lat.get_p(y_error); // calculate desired speed from lat error<br>
y_target_speed = constrain(y_target_speed,-100,100);<br>
y_rate_error = y_target_speed - y_actual_speed;<br>
<br>
p = pid_poshold_rate_lat.get_p(y_rate_error);<br>
i = pid_poshold_rate_lat.get_i(y_rate_error + y_error, dTnav);<br>
output = p + i;<br>
nav_lat = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX); <br>
<br>
// copy over I term to Nav_Rate -- if we change from poshold to RTH this will keep the wind compensation<br>
pid_nav_lon.set_integrator(pid_poshold_rate_lon.get_integrator());<br>
pid_nav_lat.set_integrator(pid_poshold_rate_lat.get_integrator());<br>
<br>
}<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH<br>
//<br>
static void GPS_calc_nav_rate(int max_speed)<br>
{<br>
int32_t course;<br>
// push us towards the original track<br>
GPS_update_crosstrack();<br>
<br>
<br>
// nav_bearing includes crosstrack<br>
float temp = (9000l - nav_bearing) * RADX100;<br>
<br>
// East / West<br>
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; <br>
x_rate_error = constrain(x_rate_error, -1000, 1000);<br>
nav_lon = pid_nav_lon.get_pid(x_rate_error, dTnav);<br>
nav_lon = constrain(nav_lon, -NAV_BANK_MAX, NAV_BANK_MAX);<br>
<br>
// North / South<br>
y_rate_error = (sin(temp) * max_speed) - y_actual_speed; <br>
y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum<br>
nav_lat = pid_nav_lat.get_pid(y_rate_error, dTnav);<br>
nav_lat = constrain(nav_lat, -NAV_BANK_MAX, NAV_BANK_MAX);<br>
<br>
// copy over I term to poshold_rate - So when arriving and entering to poshold we will have the wind compensation<br>
pid_poshold_rate_lon.set_integrator(pid_nav_lon.get_integrator());<br>
pid_poshold_rate_lat.set_integrator(pid_nav_lat.get_integrator());<br>
<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Calculating cross track error, this tries to keep the copter on a direct line <br>
// when flying to a waypoint.<br>
//<br>
static void GPS_update_crosstrack(void)<br>
{<br>
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following<br>
float temp = (target_bearing - original_target_bearing) * RADX100;<br>
crosstrack_error = sin(temp) * (wp_distance * (float)((float)i2c_dataset.nav_crosstrack_gain/100.0f)); // Meters we are off track line<br>
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);<br>
nav_bearing = wrap_36000(nav_bearing);<br>
}else{<br>
nav_bearing = target_bearing;<br>
}<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Determine desired speed when navigating towards a waypoint, also implement slow <br>
// speed rampup when starting a navigation<br>
//<br>
// |< WP Radius<br>
// 0 1 2 3 4 5 6 7 8m<br>
// ...|...|...|...|...|...|...|...|<br>
// 100 | 200 300 400cm/s<br>
// | +|+<br>
// |< we should slow to 1.5 m/s as we hit the target<br>
//<br>
static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow)<br>
{<br>
// max_speed is default 400 or 4m/s<br>
<br>
if(_slow){<br>
max_speed = min(max_speed, wp_distance / 2);<br>
max_speed = max(max_speed, 0);<br>
}else{<br>
max_speed = min(max_speed, wp_distance);<br>
max_speed = max(max_speed, i2c_dataset.nav_speed_min); // go at least 100cm/s<br>
}<br>
<br>
// limit the ramp up of the speed<br>
// waypoint_speed_gov is reset to NAV_MIN_SPEED at each new WP command<br>
if(max_speed > waypoint_speed_gov){<br>
waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms<br>
max_speed = waypoint_speed_gov;<br>
}<br>
<br>
return max_speed;<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Resets all GPS nev parameters and clears up the PID controllers. Prepares for a restarted poshold/navigation<br>
//<br>
void GPS_reset_nav()<br>
{<br>
pi_poshold_lat.reset_I();<br>
pi_poshold_lon.reset_I();<br>
pid_poshold_rate_lon.reset_I();<br>
pid_poshold_rate_lat.reset_I();<br>
pid_nav_lon.reset_I();<br>
pid_nav_lat.reset_I();<br>
nav_lon = 0;<br>
nav_lat = 0;<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Update i2c_dataset from navigation output<br>
//<br>
void GPS_update_i2c_dataset()<br>
{<br>
i2c_dataset.nav_lat = nav_lat;<br>
i2c_dataset.nav_lon = nav_lon;<br>
i2c_dataset.wp_distance = wp_distance;<br>
i2c_dataset.wp_target_bearing = target_bearing;<br>
i2c_dataset.nav_bearing = nav_bearing;<br>
<br>
}<br>
<br>
////////////////////////////////////////////////////////////////////////////////////<br>
// Utilities<br>
//<br>
int32_t wrap_18000(int32_t error)<br>
{<br>
if (error > 18000) error -= 36000;<br>
if (error < -18000) error += 36000;<br>
return error;<br>
}<br>
<br>
int32_t wrap_36000(int32_t angle)<br>
{<br>
if (angle > 36000) angle -= 36000;<br>
if (angle < 0) angle += 36000;<br>
return angle;<br>
}<br>
<br>
<br>
#if defined(NMEA)<br>
<br>
/* The latitude or longitude is coded this way in NMEA frames<br>
dddmm.mmmm coded as degrees + minutes + minute decimal<br>
This function converts this format in a unique unsigned long where 1 degree = 10 000 000<br>
I increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution<br>
with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased <br>
resolution also increased precision of nav calculations<br>
*/<br>
<br>
#define DIGIT_TO_VAL(_x) (_x - '0')<br>
uint32_t GPS_coord_to_degrees(char* s)<br>
{<br>
char *p, *q;<br>
uint8_t deg = 0, min = 0;<br>
unsigned int frac_min = 0;<br>
<br>
// scan for decimal point or end of field<br>
for (p = s; isdigit(*p); p++)<br>
;<br>
q = s;<br>
<br>
// convert degrees<br>
while ((p - q) > 2) {<br>
if (deg)<br>
deg *= 10;<br>
deg += DIGIT_TO_VAL(*q++);<br>
}<br>
// convert minutes<br>
while (p > q) {<br>
if (min)<br>
min *= 10;<br>
min += DIGIT_TO_VAL(*q++);<br>
}<br>
// convert fractional minutes<br>
// expect up to four digits, result is in<br>
// ten-thousandths of a minute<br>
if (*p == '.') {<br>
q = p + 1;<br>
for (int i = 0; i < 4; i++) {<br>
frac_min *= 10;<br>
if (isdigit(*q))<br>
frac_min += *q++ - '0';<br>
}<br>
}<br>
return deg * 10000000UL + (min * 1000000UL + frac_min*100UL) / 6;<br>
}<br>
<br>
/* This is am expandable implementation of a GPS frame decoding<br>
This should work with most of modern GPS devices configured to output NMEA frames.<br>
It assumes there are some NMEA GGA, GSA and RMC frames to decode on the serial bus<br>
Using the following data :<br>
GGA<br>
- time<br>
- latitude<br>
- longitude<br>
- GPS fix <br>
- GPS num sat (5 is enough to be +/- reliable)<br>
- GPS alt<br>
GSA<br>
- 3D fix (it could be left out since we have a 3D fix if we have more than 4 sats <br>
RMC<br>
- GPS speed over ground, it will be handy for wind compensation (future) <br>
<br>
*/<br>
<br>
#define NO_FRAME 0<br>
#define GPGGA_FRAME 1<br>
#define GPGSA_FRAME 2<br>
#define GPRMC_FRAME 3<br>
<br>
bool GPS_NMEA_newFrame(char c) {<br>
<br>
uint8_t frameOK = 0;<br>
static uint8_t param = 0, offset = 0, parity = 0;<br>
static char string[15];<br>
static uint8_t checksum_param, gps_frame = NO_FRAME;<br>
<br>
switch (c) {<br>
case '$': param = 0; offset = 0; parity = 0; <br>
break;<br>
case ',':<br>
case '*': string[offset] = 0;<br>
if (param == 0) { //frame identification<br>
gps_frame = NO_FRAME; <br>
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') gps_frame = GPGGA_FRAME;<br>
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'A') gps_frame = GPGSA_FRAME;<br>
if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') gps_frame = GPRMC_FRAME;<br>
}<br>
<br>
switch (gps_frame)<br>
{<br>
//************* GPGGA FRAME parsing<br>
case GPGGA_FRAME: <br>
switch (param)<br>
{<br>
case 1: i2c_dataset.time = (atof(string)*1000); //up to .000 s precision not needed really but the data is there anyway<br>
break;<br>
//case 2: i2c_dataset.gps_loc.lat = GPS_coord_to_degrees(string);<br>
case 2: GPS_read[LAT] = GPS_coord_to_degrees(string);<br>
break;<br>
//case 3: if (string[0] == 'S') i2c_dataset.gps_loc.lat = -i2c_dataset.gps_loc.lat;<br>
case 3: if (string[0] == 'S') GPS_read[LAT] = -GPS_read[LAT];<br>
break;<br>
//case 4: i2c_dataset.gps_loc.lon = GPS_coord_to_degrees(string);<br>
case 4: GPS_read[LON] = GPS_coord_to_degrees(string);<br>
break;<br>
//case 5: if (string[0] == 'W') i2c_dataset.gps_loc.lon = -i2c_dataset.gps_loc.lon;<br>
case 5: if (string[0] == 'W') GPS_read[LON] = -GPS_read[LON];<br>
break;<br>
case 6: i2c_dataset.status.gps2dfix = string[0] > '0';<br>
break;<br>
case 7: i2c_dataset.status.numsats = atoi(string);<br>
break;<br>
case 9: i2c_dataset.altitude = atoi(string);<br>
break;<br>
}<br>
break; <br>
//************* GPGSA FRAME parsing<br>
case GPGSA_FRAME:<br>
switch (param)<br>
{<br>
case 2: i2c_dataset.status.gps3dfix = string[0] == '3';<br>
break;<br>
}<br>
break;<br>
//************* GPGSA FRAME parsing<br>
case GPRMC_FRAME:<br>
switch(param)<br>
{<br>
case 7: i2c_dataset.ground_speed = (atof(string)*0.5144444)*10; //convert to m/s*100<br>
break; <br>
case 8: i2c_dataset.ground_course = (atof(string)*10); //Convert to degrees *10 (.1 precision)<br>
break;<br>
}<br>
<br>
break; <br>
}<br>
<br>
param++; offset = 0;<br>
if (c == '*') checksum_param=1;<br>
else parity ^= c;<br>
break;<br>
case '\r':<br>
case '\n': <br>
if (checksum_param) { //parity checksum<br>
uint8_t checksum = 16 * ((string[0]>='A') ? string[0] - 'A'+10: string[0] - '0') + ((string[1]>='A') ? string[1] - 'A'+10: string[1]-'0');<br>
if (checksum == parity) frameOK = 1;<br>
}<br>
checksum_param=0;<br>
break;<br>
default:<br>
if (offset < 15) string[offset++] = c;<br>
if (!checksum_param) parity ^= c;<br>
<br>
}<br>
if (frameOK) {<br>
lastframe_time = millis();<br>
}<br>
return frameOK && (gps_frame == GPGGA_FRAME);<br>
}<br>
#endif <br>
<br>
#if defined(UBLOX)<br>
<br>
struct ubx_header {<br>
uint8_t preamble1;<br>
uint8_t preamble2;<br>
uint8_t msg_class;<br>
uint8_t msg_id;<br>
uint16_t length;<br>
};<br>
<br>
struct ubx_nav_posllh {<br>
uint32_t time; // GPS msToW<br>
int32_t longitude;<br>
int32_t latitude;<br>
int32_t altitude_ellipsoid;<br>
int32_t altitude_msl;<br>
uint32_t horizontal_accuracy;<br>
uint32_t vertical_accuracy;<br>
};<br>
struct ubx_nav_status {<br>
uint32_t time; // GPS msToW<br>
uint8_t fix_type;<br>
uint8_t fix_status;<br>
uint8_t differential_status;<br>
uint8_t res;<br>
uint32_t time_to_first_fix;<br>
uint32_t uptime; // milliseconds<br>
};<br>
struct ubx_nav_solution {<br>
uint32_t time;<br>
int32_t time_nsec;<br>
int16_t week;<br>
uint8_t fix_type;<br>
uint8_t fix_status;<br>
int32_t ecef_x;<br>
int32_t ecef_y;<br>
int32_t ecef_z;<br>
uint32_t position_accuracy_3d;<br>
int32_t ecef_x_velocity;<br>
int32_t ecef_y_velocity;<br>
int32_t ecef_z_velocity;<br>
uint32_t speed_accuracy;<br>
uint16_t position_DOP;<br>
uint8_t res;<br>
uint8_t satellites;<br>
uint32_t res2;<br>
};<br>
struct ubx_nav_velned {<br>
uint32_t time; // GPS msToW<br>
int32_t ned_north;<br>
int32_t ned_east;<br>
int32_t ned_down;<br>
uint32_t speed_3d;<br>
uint32_t speed_2d;<br>
int32_t heading_2d;<br>
uint32_t speed_accuracy;<br>
uint32_t heading_accuracy;<br>
};<br>
<br>
enum ubs_protocol_bytes {<br>
PREAMBLE1 = 0xb5,<br>
PREAMBLE2 = 0x62,<br>
CLASS_NAV = 0x01,<br>
CLASS_ACK = 0x05,<br>
CLASS_CFG = 0x06,<br>
MSG_ACK_NACK = 0x00,<br>
MSG_ACK_ACK = 0x01,<br>
MSG_POSLLH = 0x2,<br>
MSG_STATUS = 0x3,<br>
MSG_SOL = 0x6,<br>
MSG_VELNED = 0x12,<br>
MSG_CFG_PRT = 0x00,<br>
MSG_CFG_RATE = 0x08,<br>
MSG_CFG_SET_RATE = 0x01,<br>
MSG_CFG_NAV_SETTINGS = 0x24<br>
};<br>
enum ubs_nav_fix_type {<br>
FIX_NONE = 0,<br>
FIX_DEAD_RECKONING = 1,<br>
FIX_2D = 2,<br>
FIX_3D = 3,<br>
FIX_GPS_DEAD_RECKONING = 4,<br>
FIX_TIME = 5<br>
};<br>
enum ubx_nav_status_bits {<br>
NAV_STATUS_FIX_VALID = 1<br>
};<br>
<br>
// Packet checksum accumulators<br>
static uint8_t _ck_a;<br>
static uint8_t _ck_b;<br>
<br>
// State machine state<br>
static uint8_t _step;<br>
static uint8_t _msg_id;<br>
static uint16_t _payload_length;<br>
static uint16_t _payload_counter;<br>
<br>
static bool next_fix;<br>
<br>
static uint8_t _class;<br>
<br>
// do we have new position information?<br>
static bool _new_position;<br>
<br>
// do we have new speed information?<br>
static bool _new_speed;<br>
<br>
static uint8_t _disable_counter;<br>
<br>
// Receive buffer<br>
static union {<br>
ubx_nav_posllh posllh;<br>
ubx_nav_status status;<br>
ubx_nav_solution solution;<br>
ubx_nav_velned velned;<br>
uint8_t bytes[];<br>
} _buffer;<br>
<br>
void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)<br>
{<br>
while (len--) {<br>
ck_a += *data;<br>
ck_b += ck_a;<br>
data++;<br>
}<br>
}<br>
<br>
bool GPS_UBLOX_newFrame(uint8_t data)<br>
{<br>
bool parsed = false;<br>
<br>
switch(_step) {<br>
<br>
case 1:<br>
if (PREAMBLE2 == data) {<br>
_step++;<br>
break;<br>
}<br>
_step = 0;<br>
case 0:<br>
if(PREAMBLE1 == data) _step++;<br>
break;<br>
<br>
case 2:<br>
_step++;<br>
_class = data;<br>
_ck_b = _ck_a = data; // reset the checksum accumulators<br>
break;<br>
case 3:<br>
_step++;<br>
_ck_b += (_ck_a += data); // checksum byte<br>
_msg_id = data;<br>
break;<br>
case 4:<br>
_step++;<br>
_ck_b += (_ck_a += data); // checksum byte<br>
_payload_length = data; // payload length low byte<br>
break;<br>
case 5:<br>
_step++;<br>
_ck_b += (_ck_a += data); // checksum byte<br>
<br>
_payload_length += (uint16_t)(data<<8);<br>
if (_payload_length > 512) {<br>
_payload_length = 0;<br>
_step = 0;<br>
}<br>
_payload_counter = 0; // prepare to receive payload<br>
break;<br>
case 6:<br>
_ck_b += (_ck_a += data); // checksum byte<br>
if (_payload_counter < sizeof(_buffer)) {<br>
_buffer.bytes[_payload_counter] = data;<br>
}<br>
if (++_payload_counter == _payload_length)<br>
_step++;<br>
break;<br>
case 7:<br>
_step++;<br>
if (_ck_a != data) _step = 0; // bad checksum<br>
break;<br>
case 8:<br>
_step = 0;<br>
if (_ck_b != data) break; // bad checksum<br>
lastframe_time = millis();<br>
if (UBLOX_parse_gps()) { parsed = true; }<br>
} //end switch<br>
return parsed;<br>
}<br>
<br>
bool UBLOX_parse_gps(void)<br>
{<br>
<br>
switch (_msg_id) {<br>
case MSG_POSLLH:<br>
i2c_dataset.time = _buffer.posllh.time;<br>
GPS_read[LON] = _buffer.posllh.longitude;<br>
GPS_read[LAT] = _buffer.posllh.latitude;<br>
i2c_dataset.altitude = _buffer.posllh.altitude_msl / 10 /100; //alt in m<br>
i2c_dataset.status.gps3dfix = next_fix;<br>
_new_position = true;<br>
break;<br>
case MSG_STATUS:<br>
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);<br>
if (!next_fix) i2c_dataset.status.gps3dfix = false;<br>
break;<br>
case MSG_SOL:<br>
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);<br>
if (!next_fix) i2c_dataset.status.gps3dfix = false;<br>
i2c_dataset.status.numsats = _buffer.solution.satellites;<br>
//GPS_hdop = _buffer.solution.position_DOP;<br>
//debug[3] = GPS_hdop;<br>
break;<br>
case MSG_VELNED:<br>
//speed_3d = _buffer.velned.speed_3d; // cm/s<br>
i2c_dataset.ground_speed = _buffer.velned.speed_2d; // cm/s<br>
i2c_dataset.ground_course = (uint16_t)(_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10<br>
_new_speed = true;<br>
break;<br>
default:<br>
return false;<br>
}<br>
<br>
// we only return true when we get new position and speed data<br>
// this ensures we don't use stale data<br>
if (_new_position && _new_speed) {<br>
_new_speed = _new_position = false;<br>
return true;<br>
}<br>
return false;<br>
}<br>
<br>
<br>
#endif //UBLOX<br>
<br>
#if defined(MTK)<br>
struct diyd_mtk_msg {<br>
int32_t latitude;<br>
int32_t longitude;<br>
int32_t altitude;<br>
int32_t ground_speed;<br>
int32_t ground_course;<br>
uint8_t satellites;<br>
uint8_t fix_type;<br>
uint32_t utc_date;<br>
uint32_t utc_time;<br>
uint16_t hdop;<br>
};<br>
// #pragma pack(pop)<br>
enum diyd_mtk_fix_type {<br>
FIX_NONE = 1,<br>
FIX_2D = 2,<br>
FIX_3D = 3<br>
};<br>
<br>
enum diyd_mtk_protocol_bytes {<br>
PREAMBLE1 = 0xd0,<br>
PREAMBLE2 = 0xdd,<br>
};<br>
<br>
// Packet checksum accumulators<br>
uint8_t _ck_a;<br>
uint8_t _ck_b;<br>
<br>
// State machine state<br>
uint8_t _step;<br>
uint8_t _payload_counter;<br>
<br>
// Time from UNIX Epoch offset<br>
long _time_offset;<br>
bool _offset_calculated;<br>
<br>
// Receive buffer<br>
union {<br>
diyd_mtk_msg msg;<br>
uint8_t bytes[];<br>
} _buffer;<br>
<br>
inline long _swapl(const void *bytes)<br>
{<br>
const uint8_t *b = (const uint8_t *)bytes;<br>
union {<br>
long v;<br>
uint8_t b[4];<br>
} u;<br>
<br>
u.b[0] = b[3];<br>
u.b[1] = b[2];<br>
u.b[2] = b[1];<br>
u.b[3] = b[0];<br>
<br>
return(u.v);<br>
}<br>
<br>
bool GPS_MTK_newFrame(uint8_t data)<br>
{<br>
bool parsed = false;<br>
<br>
restart:<br>
switch(_step) {<br>
<br>
// Message preamble, class, ID detection<br>
//<br>
// If we fail to match any of the expected bytes, we<br>
// reset the state machine and re-consider the failed<br>
// byte as the first byte of the preamble. This<br>
// improves our chances of recovering from a mismatch<br>
// and makes it less likely that we will be fooled by<br>
// the preamble appearing as data in some other message.<br>
//<br>
case 0:<br>
if(PREAMBLE1 == data)<br>
_step++;<br>
break;<br>
case 1:<br>
if (PREAMBLE2 == data) {<br>
_step++;<br>
break;<br>
}<br>
_step = 0;<br>
goto restart;<br>
case 2:<br>
if (sizeof(_buffer) == data) {<br>
_step++;<br>
_ck_b = _ck_a = data; // reset the checksum accumulators<br>
_payload_counter = 0;<br>
} else {<br>
_step = 0; // reset and wait for a message of the right class<br>
goto restart;<br>
}<br>
break;<br>
<br>
// Receive message data<br>
//<br>
case 3:<br>
_buffer.bytes[_payload_counter++] = data;<br>
_ck_b += (_ck_a += data);<br>
if (_payload_counter == sizeof(_buffer))<br>
_step++;<br>
break;<br>
<br>
// Checksum and message processing<br>
//<br>
case 4:<br>
_step++;<br>
if (_ck_a != data) {<br>
_step = 0;<br>
}<br>
break;<br>
case 5:<br>
_step = 0;<br>
if (_ck_b != data) {<br>
break;<br>
}<br>
lastframe_time = millis();<br>
i2c_dataset.status.gps3dfix = _buffer.msg.fix_type == FIX_3D;<br>
GPS_read[LAT] = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise<br>
GPS_read[LON] = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise<br>
i2c_dataset.altitude = _buffer.msg.altitude /100;<br>
i2c_dataset.ground_speed = _buffer.msg.ground_speed;<br>
i2c_dataset.ground_course = _buffer.msg.ground_course;<br>
i2c_dataset.status.numsats = _buffer.msg.satellites;<br>
//GPS_hdop = _buffer.msg.hdop;<br>
parsed = true;<br>
//GPS_Present = 1;<br>
}<br>
return parsed;<br>
}<br>
#endif //MTK<br>
<br>
<br>
<br>
//////////////////////////////////////////////////////////////////////////////////////<br>
// I2C handlers<br>
// Handler for requesting data<br>
//<br>
void requestEvent()<br>
{<br>
if (receivedCommands[0] >= I2C_GPS_GROUND_SPEED) i2c_dataset.status.new_data = 0; //Accessing gps data, switch new_data_flag;<br>
//Write data from the requested data register position<br>
Wire.write((uint8_t *)&i2c_dataset+receivedCommands[0],32); //Write up to 32 byte, since master is responsible for reading and sending NACK<br>
//32 byte limit is in the Wire library, we have to live with it unless writing our own wire library<br>
<br>
}<br>
<br>
//Handler for receiving data<br>
void receiveEvent(int bytesReceived)<br>
{<br>
uint8_t *ptr;<br>
for (int a = 0; a < bytesReceived; a++) {<br>
if (a < MAX_SENT_BYTES) {<br>
receivedCommands[a] = Wire.read();<br>
} else {<br>
Wire.read(); // if we receive more data then allowed just throw it away<br>
}<br>
}<br>
<br>
if (receivedCommands[0] == I2C_GPS_COMMAND) { new_command = receivedCommands[1]; return; } //Just one byte, ignore all others<br>
<br>
if(bytesReceived == 1 && (receivedCommands[0] < REG_MAP_SIZE)) { return; } //read command from a given register<br>
if(bytesReceived == 1 && (receivedCommands[0] >= REG_MAP_SIZE)){ //Addressing over the reg_map fallback to first byte<br>
receivedCommands[0] = 0x00;<br>
return;<br>
}<br>
//More than 1 byte was received, so there is definitely some data to write into a register<br>
//Check for writeable registers and discard data is it's not writeable<br>
<br>
if ((receivedCommands[0]>=I2C_GPS_CROSSTRACK_GAIN) && (receivedCommands[0]<=REG_MAP_SIZE)) { //Writeable registers above I2C_GPS_WP0<br>
ptr = (uint8_t *)&i2c_dataset+receivedCommands[0];<br>
for (int a = 1; a < bytesReceived; a++) { *ptr++ = receivedCommands[a]; }<br>
}<br>
}<br>
<br>
<br>
void blink_update()<br>
{<br>
uint32_t now = millis();<br>
if(_statusled_timer < now) {<br>
if(lastframe_time+2000 < now) {<br>
// no gps communication <br>
_statusled_state = !_statusled_state;<br>
digitalWrite(13, _statusled_state ? HIGH : LOW); // set the LED off<br>
_statusled_timer = now + 1000;<br>
return;<br>
}<br>
<br>
if(_statusled_blinks==0) {<br>
if(i2c_dataset.status.gps3dfix == 1)<br>
_statusled_blinks=3;<br>
else if(i2c_dataset.status.gps2dfix == 1)<br>
_statusled_blinks=2;<br>
else<br>
_statusled_blinks=1; <br>
}<br>
<br>
if(_statusled_state) {<br>
_statusled_blinks--;<br>
_statusled_state = false;<br>
_statusled_timer = now + ((_statusled_blinks>0) ? BLINK_INTERVAL : 1000);<br>
digitalWrite(13, LOW); // set the LED off<br>
} else {<br>
_statusled_state = true;<br>
_statusled_timer = now + BLINK_INTERVAL;<br>
digitalWrite(13, HIGH); // set the LED on<br>
}<br>
}<br>
}<br>
<br>
<br>
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////<br>
// Setup<br>
//<br>
void setup() {<br>
<br>
uint8_t i;<br>
<br>
delay(3000); //lets some time to GPS module to init<br>
Serial.begin(GPS_SERIAL_SPEED); // Start with GPS native speed<br>
<br>
#if defined(MTK)<br>
/* Using the AXN 1.51 firmware which defaults at 1Hz/38400 but supports binary protocol<br>
* First connect to it with 38400, then set the speed to 115200 <br>
* and set the update rate to 10Hz<br>
* and finally switch to Binary mode<br>
*/<br>
Serial.write("$PMTK251,115200*1F\r\n");<br>
delay(300);<br>
Serial.end();<br>
<br>
Serial.begin(115200);<br>
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");<br>
delay(1000);<br>
Serial.write("$PGCMD,16,0,0,0,0,0*6A\r\n");<br>
delay(300);<br>
Serial.write("$PMTK220,100*2F\r\n");<br>
#endif<br>
<br>
#if defined(UBLOX) <br>
//set GPS dynamic platform to "pedestrian" seems to be best for copter<br>
PROGMEM prog_uchar conf2[]={0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4F, 0x82};<br>
Serial.write (conf2,sizeof(conf2)); <br>
<br>
//disable all default NMEA messages<br>
PROGMEM prog_uchar conf3[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x05, 0x00, 0xFF, 0x19};<br>
Serial.write (conf3,sizeof(conf3));<br>
PROGMEM prog_uchar conf5[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x03, 0x00, 0xFD, 0x15};<br>
Serial.write (conf5,sizeof(conf5));<br>
PROGMEM prog_uchar conf6[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x01, 0x00, 0xFB, 0x11};<br>
Serial.write (conf6,sizeof(conf6));<br>
PROGMEM prog_uchar conf7[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x00, 0x00, 0xFA, 0x0F};<br>
Serial.write (conf7,sizeof(conf7));<br>
PROGMEM prog_uchar conf8[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x02, 0x00, 0xFC, 0x13};<br>
Serial.write (conf8,sizeof(conf8));<br>
PROGMEM prog_uchar conf9[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0xF0, 0x04, 0x00, 0xFE, 0x17}; <br>
Serial.write (conf9,sizeof(conf9));<br>
<br>
//enable UBX messages POSLLH, SOL, STATUS and VELNED as is in EOSBandi's config file<br>
PROGMEM prog_uchar conf10[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47};<br>
Serial.write (conf10,sizeof(conf10));<br>
PROGMEM prog_uchar conf11[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49};<br>
Serial.write (conf11,sizeof(conf11));<br>
PROGMEM prog_uchar conf12[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x06, 0x01, 0x12, 0x4F};<br>
Serial.write (conf12,sizeof(conf12));<br>
PROGMEM prog_uchar conf13[]={0xB5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x12, 0x01, 0x1E, 0x67};<br>
Serial.write (conf13,sizeof(conf13));<br>
<br>
//set WAAS to EGNOS<br>
PROGMEM prog_uchar conf14[]={0xB5,0x62,0x06,0x16,0x08,0x00,0x03,0x07,0x03,0x00,0x51,0x08,0x00,0x00,0x8A,0x41};<br>
Serial.write (conf14,sizeof(conf14));<br>
<br>
//Set rate to 10 Hz<br>
PROGMEM prog_uchar conf16[]={0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12};<br>
Serial.write (conf16,sizeof(conf16));<br>
delay(100);<br>
<br>
// port speed to 115200<br>
PROGMEM prog_uchar conf1[]={0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC4, 0x96};<br>
<br>
Serial.write (conf1, sizeof(conf1)); <br>
delay(100); <br>
Serial.end();<br>
Serial.begin(115200);<br>
while (Serial.available()) Serial.read();<br>
#endif<br>
<br>
//Init i2c_dataset;<br>
uint8_t *ptr = (uint8_t *)&i2c_dataset;<br>
for (i=0;i<sizeof(i2c_dataset);i++) { *ptr = 0; ptr++;}<br>
<br>
//Set up default parameters<br>
i2c_dataset.sw_version = VERSION;<br>
<br>
i2c_dataset.nav_crosstrack_gain = CROSSTRACK_GAIN; // Crosstrack gain = 1<br>
i2c_dataset.nav_speed_min = NAV_SPEED_MIN; // cm/s<br>
i2c_dataset.nav_speed_max = NAV_SPEED_MAX; // cm/s<br>
i2c_dataset.nav_bank_max = NAV_BANK_MAX; // 30deg<br>
i2c_dataset.wp_radius = WP_RADIUS; //cm -> 2m<br>
<br>
i2c_dataset.poshold_p = POSHOLD_P * 100;<br>
i2c_dataset.poshold_i = POSHOLD_I * 100;<br>
i2c_dataset.poshold_imax = POSHOLD_IMAX;<br>
<br>
i2c_dataset.poshold_rate_p = POSHOLD_RATE_P * 10;<br>
i2c_dataset.poshold_rate_i = POSHOLD_RATE_I * 100;<br>
i2c_dataset.poshold_rate_d = POSHOLD_RATE_D * 1000;<br>
i2c_dataset.poshold_rate_imax = POSHOLD_RATE_IMAX;<br>
<br>
i2c_dataset.nav_p = NAV_P * 10;<br>
i2c_dataset.nav_i = NAV_I * 100;<br>
i2c_dataset.nav_d = NAV_D * 1000;<br>
i2c_dataset.nav_imax = NAV_IMAX;<br>
<br>
i2c_dataset.nav_flags = 0x80 + 0x40; // GPS filter and low speed filters are on<br>
<br>
//Start I2C communication routines<br>
Wire.begin(I2C_ADDRESS); // DO NOT FORGET TO COMPILE WITH 400KHz!!! else change TWBR Speed to 100khz on Host !!! Address 0x40 write 0x41 read<br>
Wire.onRequest(requestEvent); // Set up event handlers<br>
Wire.onReceive(receiveEvent);<br>
<br>
<br>
}<br>
<br>
/******************************************************************************************************************/<br>
/******************************************************* Main loop ************************************************/<br>
/******************************************************************************************************************/<br>
void loop() {<br>
<br>
static uint8_t GPS_fix_home;<br>
static uint8_t _command_wp;<br>
static uint8_t _command;<br>
static uint32_t _watchdog_timer = 0;<br>
uint8_t axis;<br>
uint16_t fraction3[2];<br>
<br>
while (Serial.available()) {<br>
#if defined(NMEA)<br>
if (GPS_NMEA_newFrame(Serial.read())) {<br>
#endif <br>
#if defined(UBLOX)<br>
if (GPS_UBLOX_newFrame(Serial.read())) {<br>
#endif<br>
#if defined(MTK)<br>
if (GPS_MTK_newFrame(Serial.read())) {<br>
#endif<br>
<br>
// We have a valid GGA frame and we have lat and lon in GPS_read_lat and GPS_read_lon, apply moving average filter<br>
// this is a little bit tricky since the 1e7/deg precision easily overflow a long, so we apply the filter to the fractions<br>
// only, and strip the full degrees part. This means that we have to disable the filter if we are very close to a degree line<br>
<br>
if (i2c_dataset.nav_flags & I2C_NAV_FLAG_GPS_FILTER) { //is filtering switched on ?<br>
<br>
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;<br>
<br>
for (axis = 0; axis< 2; axis++) {<br>
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t<br>
<br>
// How close we are to a degree line ? its the first three digits from the fractions of degree<br>
//Check if we are close to a degree line, if yes, disable averaging,<br>
fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000;<br>
<br>
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];<br>
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000); <br>
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];<br>
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);<br>
} <br>
<br>
if ( nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode...<br>
if ( fraction3[LAT]>1 && fraction3[LAT]<999 ) i2c_dataset.gps_loc.lat = GPS_filtered[LAT]; else i2c_dataset.gps_loc.lat = GPS_read[LAT];<br>
if ( fraction3[LON]>1 && fraction3[LON]<999 ) i2c_dataset.gps_loc.lon = GPS_filtered[LON]; else i2c_dataset.gps_loc.lon = GPS_read[LON]; <br>
} else {<br>
i2c_dataset.gps_loc.lat = GPS_read[LAT];<br>
i2c_dataset.gps_loc.lon = GPS_read[LON];<br>
}<br>
} else { // ignore filtering since it switced off in the nav_flags<br>
i2c_dataset.gps_loc.lat = GPS_read[LAT];<br>
i2c_dataset.gps_loc.lon = GPS_read[LON];<br>
}<br>
<br>
if (i2c_dataset.status.gps3dfix == 1 && i2c_dataset.status.numsats >= 5) {<br>
<br>
//copy the gps coordinates to variables used for calculations<br>
GPS_latitude = i2c_dataset.gps_loc.lat;<br>
GPS_longitude = i2c_dataset.gps_loc.lon;<br>
<br>
// It's just for safety since home position must be set from the host<br>
if (GPS_fix_home == 0) {<br>
GPS_fix_home = 1;<br>
i2c_dataset.gps_wp[0].position.lat = GPS_latitude;<br>
i2c_dataset.gps_wp[0].position.lon = GPS_longitude;<br>
GPS_calc_longitude_scaling(GPS_latitude); //need an initial value for distance and bearing calc<br>
}<br>
//dTnav calculation<br>
//Time for calculating x,y speed and navigation pids<br>
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;<br>
nav_loopTimer = millis();<br>
// prevent runup from bad GPS<br>
dTnav = min(dTnav, 1.0); <br>
<br>
_watchdog_timer = millis(); //Reset watchdog timer<br>
<br>
//calculate distance and bearings for gui and other stuff continously this is independent from navigation<br>
i2c_dataset.distance_to_home = GPS_distance_cm(GPS_latitude,GPS_longitude,i2c_dataset.gps_wp[0].position.lat,i2c_dataset.gps_wp[0].position.lon);<br>
i2c_dataset.home_to_copter_bearing = GPS_bearing(i2c_dataset.gps_wp[0].position.lat,i2c_dataset.gps_wp[0].position.lon,GPS_latitude,GPS_longitude);<br>
//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating<br>
GPS_calc_velocity(GPS_latitude,GPS_longitude);<br>
<br>
if (GPSMode != 0){ //ok we are navigating <br>
//do gps nav calculations here <br>
<br>
#if defined(GPS_LEAD_FILTER)<br>
wp_distance = GPS_distance_cm(GPS_lead_latitude,GPS_lead_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
target_bearing = GPS_bearing(GPS_lead_latitude,GPS_lead_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
GPS_calc_location_error(GPS_WP_latitude,GPS_WP_longitude,GPS_lead_latitude,GPS_lead_longitude);<br>
#else <br>
wp_distance = GPS_distance_cm(GPS_latitude,GPS_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
target_bearing = GPS_bearing(GPS_latitude,GPS_longitude,GPS_WP_latitude,GPS_WP_longitude);<br>
GPS_calc_location_error(GPS_WP_latitude,GPS_WP_longitude,GPS_latitude,GPS_longitude);<br>
#endif<br>
<br>
switch (nav_mode) {<br>
case NAV_MODE_POSHOLD: <br>
//Desired output is in nav_lat and nav_lon where 1deg inclination is 100 <br>
GPS_calc_poshold(long_error, lat_error);<br>
break;<br>
case NAV_MODE_WP:<br>
int16_t speed = GPS_calc_desired_speed(i2c_dataset.nav_speed_max, true); //slow navigation <br>
// use error as the desired rate towards the target<br>
GPS_calc_nav_rate(speed);<br>
<br>
// Are we there yet ?(within 2 meters of the destination)<br>
if ((wp_distance <= i2c_dataset.wp_radius) || check_missed_wp()){ //if yes switch to poshold mode<br>
nav_mode = NAV_MODE_POSHOLD;<br>
//set reached flag<br>
i2c_dataset.status.wp_reached = 1;<br>
} <br>
break; <br>
} //switch nav mode<br>
} // if GPSmode!=0 <br>
// update i2c dataset from nav <br>
GPS_update_i2c_dataset();<br>
} else { // we does not have 3d fix or numsats less than 5 , stop navigation<br>
nav_lat = 0;<br>
nav_lon = 0;<br>
GPSMode = GPSMODE_NONAV;<br>
nav_mode = NAV_MODE_NONE;<br>
wp_distance = 0;<br>
i2c_dataset.distance_to_home = 0;<br>
i2c_dataset.home_to_copter_bearing = 0;<br>
GPS_update_i2c_dataset();<br>
}<br>
// have new data at this point anyway<br>
i2c_dataset.status.new_data = 1;<br>
} // new frame<br>
} //while <br>
<br>
blink_update(); <br>
<br>
//check watchdog timer, after 1200ms without valid packet, assume that gps communication is lost.<br>
if (_watchdog_timer != 0)<br>
{ <br>
if (_watchdog_timer+1200 < millis()) <br>
{<br>
i2c_dataset.status.gps2dfix = 0;<br>
i2c_dataset.status.gps3dfix = 0;<br>
i2c_dataset.status.numsats = 0;<br>
i2c_dataset.gps_loc.lat = 0;<br>
i2c_dataset.gps_loc.lon = 0;<br>
nav_lat = 0;<br>
nav_lon = 0;<br>
GPS_update_i2c_dataset();<br>
_watchdog_timer = 0;<br>
i2c_dataset.status.new_data = 1;<br>
}<br>
}<br>
<br>
//Check for new incoming command on I2C<br>
if (new_command!=0) {<br>
_command = new_command; //save command byte for processing<br>
new_command = 0; //clear it<br>
<br>
_command_wp = (_command & 0xF0) >> 4; //mask 4 MSB bits and shift down<br>
_command = _command & 0x0F; //empty 4MSB bits<br>
<br>
switch (_command) {<br>
case I2C_GPS_COMMAND_POSHOLD:<br>
GPS_set_next_wp(16); //wp16 is a virtual one, means current location<br>
GPSMode = GPSMODE_HOLD;<br>
nav_mode = NAV_MODE_POSHOLD;<br>
i2c_dataset.status.new_data = 0; //invalidate current dataset<br>
break; <br>
case I2C_GPS_COMMAND_START_NAV:<br>
GPS_set_next_wp(_command_wp);<br>
GPSMode = GPSMODE_WP;<br>
nav_mode = NAV_MODE_WP;<br>
i2c_dataset.status.new_data = 0; //invalidate current dataset<br>
break; <br>
case I2C_GPS_COMMAND_SET_WP:<br>
i2c_dataset.gps_wp[_command_wp].position.lat = GPS_latitude;<br>
i2c_dataset.gps_wp[_command_wp].position.lon = GPS_longitude;<br>
break;<br>
case I2C_GPS_COMMAND_UPDATE_PIDS:<br>
pi_poshold_lat.kP((float)i2c_dataset.poshold_p/100.0f);<br>
pi_poshold_lon.kP((float)i2c_dataset.poshold_p/100.0f);<br>
pi_poshold_lat.kI((float)i2c_dataset.poshold_i/100.0f);<br>
pi_poshold_lon.kI((float)i2c_dataset.poshold_i/100.0f);<br>
pi_poshold_lat.imax(i2c_dataset.poshold_imax*100);<br>
pi_poshold_lon.imax(i2c_dataset.poshold_imax*100);<br>
<br>
pid_poshold_rate_lat.kP((float)i2c_dataset.poshold_rate_p/10.0f);<br>
pid_poshold_rate_lon.kP((float)i2c_dataset.poshold_rate_p/10.0f);<br>
pid_poshold_rate_lat.kI((float)i2c_dataset.poshold_rate_i/100.0f);<br>
pid_poshold_rate_lon.kI((float)i2c_dataset.poshold_rate_i/100.0f);<br>
pid_poshold_rate_lat.kD((float)i2c_dataset.poshold_rate_d/1000.0f);<br>
pid_poshold_rate_lon.kD((float)i2c_dataset.poshold_rate_d/1000.0f);<br>
pid_poshold_rate_lat.imax(i2c_dataset.poshold_rate_imax*100);<br>
pid_poshold_rate_lon.imax(i2c_dataset.poshold_rate_imax*100);<br>
<br>
pid_nav_lat.kP((float)i2c_dataset.nav_p/10.0f);<br>
pid_nav_lon.kP((float)i2c_dataset.nav_p/10.0f);<br>
pid_nav_lat.kI((float)i2c_dataset.nav_i/100.0f);<br>
pid_nav_lon.kI((float)i2c_dataset.nav_i/100.0f);<br>
pid_nav_lat.kD((float)i2c_dataset.nav_d/1000.0f);<br>
pid_nav_lon.kD((float)i2c_dataset.nav_d/1000.0f);<br>
pid_nav_lat.imax(i2c_dataset.nav_imax*100);<br>
pid_nav_lon.imax(i2c_dataset.nav_imax*100);<br>
break; <br>
case I2C_GPS_COMMAND_STOP_NAV:<br>
GPS_reset_nav();<br>
GPSMode = GPSMODE_NONAV;<br>
nav_mode = NAV_MODE_NONE;<br>
GPS_update_i2c_dataset();<br>
i2c_dataset.status.new_data = 1;<br>
break;<br>
<br>
} //switch <br>
}<br>
}<br>
<br>
<br>
-----------------------------------------------------------------------------------------------------------<br>
GPS - Config.h:<br>
/*************************************************************************************************/<br>
/**** CONFIGURABLE PARAMETERS ****/<br>
/*************************************************************************************************/<br>
<br>
/* this file consists of several sections<br>
* to create a working combination you must at least make your choices in section 1.<br>
* 1 - BASIC SETUP - you must select an option in every block.<br>
* this assumes you have 4 channels connected to your board with standard ESCs and servos.<br>
* 2 - COPTER TYPE SPECIFIC OPTIONS - you likely want to check for options for your copter type<br>
* 3 - RC SYSTEM SETUP<br>
* 4 - ALTERNATE CPUs & BOARDS - if you have<br>
* 5 - ALTERNATE SETUP - select alternate RX (SBUS, PPM, etc.), alternate ESC-range, etc. here<br>
* 6 - OPTIONAL FEATURES - enable nice to have features here (LCD, telemetry, battery monitor etc.)<br>
* 7 - TUNING & DEVELOPER - if you know what you are doing; you have been warned<br>
*/<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 1 - BASIC SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/************************** The type of multicopter ****************************/<br>
//#define GIMBAL<br>
//#define BI<br>
//#define TRI<br>
//#define QUADP<br>
#define QUADX<br>
//#define Y4<br>
//#define Y6<br>
//#define HEX6<br>
//#define HEX6X<br>
//#define OCTOX8<br>
//#define OCTOFLATP<br>
//#define OCTOFLATX<br>
//#define FLYING_WING<br>
//#define VTAIL4<br>
//#define AIRPLANE<br>
//#define SINGLECOPTER<br>
//#define DUALCOPTER<br>
//#define HELI_120_CCPM<br>
//#define HELI_90_DEG<br>
<br>
/**************************** Motor minthrottle *******************************/<br>
/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)<br>
This is the minimum value that allow motors to run at a idle speed */<br>
#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A<br>
//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A<br>
//#define MINTHROTTLE 1064 // special ESC (simonk)<br>
//#define MINTHROTTLE 1150<br>
<br>
/**************************** Motor maxthrottle *******************************/<br>
/* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */<br>
#define MAXTHROTTLE 1850<br>
<br>
/**************************** Mincommand *******************************/<br>
/* this is the value for the ESCs when they are not armed<br>
in some cases, this value must be lowered down to 900 for some specific ESCs, otherwise they failed to initiate */<br>
#define MINCOMMAND 1000<br>
<br>
/********************************** I2C speed ************************************/<br>
#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP<br>
//#define I2C_SPEED 400000L //400kHz fast mode, it works only with some WMP clones<br>
<br>
/*************************** Internal i2c Pullups ********************************/<br>
/* enable internal I2C pull ups (in most cases it is better to use external pullups) */<br>
//#define INTERNAL_I2C_PULLUPS<br>
<br>
/**************************************************************************************/<br>
/***************** boards and sensor definitions ******************/<br>
/**************************************************************************************/<br>
<br>
/*************************** Combined IMU Boards ********************************/<br>
/* if you use a specific sensor board:<br>
please submit any correction to this list.<br>
Note from Alex: I only own some boards, for other boards, I'm not sure, the info was gathered via rc forums, be cautious */<br>
//#define FFIMUv1 // first 9DOF+baro board from Jussi, with HMC5843 <- confirmed by Alex<br>
//#define FFIMUv2 // second version of 9DOF+baro board from Jussi, with HMC5883 <- confirmed by Alex<br>
//#define FREEIMUv1 // v0.1 & v0.2 & v0.3 version of 9DOF board from Fabio<br>
//#define FREEIMUv03 // FreeIMU v0.3 and v0.3.1<br>
//#define FREEIMUv035 // FreeIMU v0.3.5 no baro<br>
//#define FREEIMUv035_MS // FreeIMU v0.3.5_MS <- confirmed by Alex<br>
//#define FREEIMUv035_BMP // FreeIMU v0.3.5_BMP<br>
//#define FREEIMUv04 // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA <- confirmed by Alex<br>
//#define FREEIMUv043 // same as FREEIMUv04 with final MPU6050 (with the right ACC scale)<br>
//#define NANOWII // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex<br>
//#define PIPO // 9DOF board from erazz<br>
//#define QUADRINO // full FC board 9DOF+baro board from witespy with BMP085 baro <- confirmed by Alex<br>
//#define QUADRINO_ZOOM // full FC board 9DOF+baro board from witespy second edition<br>
//#define QUADRINO_ZOOM_MS// full FC board 9DOF+baro board from witespy second edition <- confirmed by Alex<br>
//#define ALLINONE // full FC board or standalone 9DOF+baro board from CSG_EU<br>
//#define AEROQUADSHIELDv2<br>
//#define ATAVRSBIN1 // Atmel 9DOF (Contribution by EOSBandi). requires 3.3V power.<br>
//#define SIRIUS // Sirius Navigator IMU <- confirmed by Alex<br>
//#define SIRIUS600 // Sirius Navigator IMU using the WMP for the gyro<br>
//#define MINIWII // Jussi's MiniWii Flight Controller <- confirmed by Alex<br>
//#define CITRUSv2_1 // CITRUS from qcrc.ca<br>
//#define CHERRY6DOFv1_0<br>
//#define DROTEK_10DOF // Drotek 10DOF with ITG3200, BMA180, HMC5883, BMP085, w or w/o LLC<br>
//#define DROTEK_10DOF_MS // Drotek 10DOF with ITG3200, BMA180, HMC5883, MS5611, LLC<br>
//#define DROTEK_6DOFv2 // Drotek 6DOF v2<br>
//#define DROTEK_6DOF_MPU // Drotek 6DOF with MPU6050<br>
//#define DROTEK_10DOF_MPU//<br>
//#define MONGOOSE1_0 // mongoose 1.0 <a href="http://store.ckdevices.com/" target="_blank" rel="nofollow">http://store.ckdevices.com/</a><br>
//#define CRIUS_LITE // Crius MultiWii Lite<br>
#define CRIUS_SE // Crius MultiWii SE<br>
//#define OPENLRSv2MULTI // OpenLRS v2 Multi Rc Receiver board including ITG3205 and ADXL345<br>
//#define BOARD_PROTO_1 // with MPU6050 + HMC5883L + MS baro<br>
//#define BOARD_PROTO_2 // with MPU6050 + slave MAG3110 + MS baro<br>
//#define GY_80 // Chinese 10 DOF with L3G4200D ADXL345 HMC5883L BMP085, LLC<br>
//#define GY_85 // Chinese 9 DOF with ITG3205 ADXL345 HMC5883L LLC<br>
//#define GY_86 // Chinese 10 DOF with MPU6050 HMC5883L MS5611, LLC<br>
//#define INNOVWORKS_10DOF // with ITG3200, BMA180, HMC5883, BMP085 available here <a href="http://www.diymulticopter.com" target="_blank" rel="nofollow">http://www.diymulticopter.com</a><br>
//#define INNOVWORKS_6DOF // with ITG3200, BMA180 available here <a href="http://www.diymulticopter.com" target="_blank" rel="nofollow">http://www.diymulticopter.com</a><br>
//#define PROTO_DIY // 10DOF mega board<br>
//#define IOI_MINI_MULTIWII// <a href="http://www.bambucopter.com" target="_blank" rel="nofollow">www.bambucopter.com</a><br>
//#define Bobs_6DOF_V1 // BobsQuads 6DOF V1 with ITG3200 & BMA180<br>
//#define Bobs_9DOF_V1 // BobsQuads 9DOF V1 with ITG3200, BMA180 & HMC5883L<br>
//#define Bobs_10DOF_BMP_V1 // BobsQuads 10DOF V1 with ITG3200, BMA180, HMC5883L & BMP180 - BMP180 is software compatible with BMP085<br>
//#define FLYDUINO_MPU<br>
//#define CRIUS_AIO_PRO_V1<br>
<br>
/*************************** independent sensors ********************************/<br>
/* leave it commented if you already checked a specific board above */<br>
/* I2C gyroscope */<br>
//#define WMP<br>
#define ITG3200<br>
//#define L3G4200D<br>
//#define MPU6050 //combo + ACC<br>
<br>
/* I2C accelerometer */<br>
//#define NUNCHUCK // if you want to use the nunckuk connected to a WMP<br>
//#define MMA7455<br>
//#define ADXL345<br>
//#define BMA020<br>
#define BMA180<br>
//#define NUNCHACK // if you want to use the nunckuk as a standalone I2C ACC without WMP<br>
//#define LIS3LV02<br>
//#define LSM303DLx_ACC<br>
<br>
/* I2C barometer */<br>
#define BMP085<br>
//#define MS561101BA<br>
<br>
/* I2C magnetometer */<br>
//#define HMC5843<br>
#define HMC5883<br>
//#define AK8975<br>
//#define MAG3110<br>
<br>
/* Sonar */ // for visualization purpose currently - no control code behind<br>
//#define SRF02 // use the Devantech SRF i2c sensors<br>
//#define SRF08<br>
//#define SRF10<br>
//#define SRF23<br>
<br>
/* ADC accelerometer */ // for 5DOF from sparkfun, uses analog PIN A1/A2/A3<br>
//#define ADCACC<br>
<br>
/* individual sensor orientation */<br>
//#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z;}<br>
//#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z;}<br>
//#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;}<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 2 - COPTER TYPE SPECIFIC OPTIONS *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/******************************** TRI *********************************/<br>
#define YAW_DIRECTION 1<br>
//#define YAW_DIRECTION -1 // if you want to reverse the yaw correction direction<br>
/* you can change the tricopter servo travel here */<br>
#define TRI_YAW_CONSTRAINT_MIN 1020<br>
#define TRI_YAW_CONSTRAINT_MAX 2000<br>
#define TRI_YAW_MIDDLE 1500 // tail servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
<br>
/******************************** ARM/DISARM *********************************/<br>
/* optionally disable stick combinations to arm/disarm the motors.<br>
* In most cases one of the two options to arm/disarm via TX stick is sufficient */<br>
#define ALLOW_ARM_DISARM_VIA_TX_YAW<br>
//#define ALLOW_ARM_DISARM_VIA_TX_ROLL<br>
<br>
/*********************** Cam Stabilisation ***********************/<br>
/* The following lines apply only for a pitch/roll tilt stabilization system. Uncomment the first or second line to activate it */<br>
//#define SERVO_MIX_TILT<br>
//#define SERVO_TILT<br>
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020<br>
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000<br>
#define TILT_PITCH_MIDDLE 1500 //servo neutral value<br>
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement<br>
#define TILT_ROLL_MIN 1020<br>
#define TILT_ROLL_MAX 2000<br>
#define TILT_ROLL_MIDDLE 1500<br>
#define TILT_ROLL_PROP 10<br>
<br>
/* camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */<br>
//#define CAMTRIG<br>
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo<br>
#define CAM_SERVO_LOW 1020 // the position of LOW state servo<br>
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms<br>
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms<br>
<br>
/*********************** Flying Wing ***********************/<br>
/* you can change change servo orientation and servo min/max values here<br>
valid for all flight modes, even passThrough mode<br>
need to setup servo directions here; no need to swap servos amongst channels at rx */<br>
#define PITCH_DIRECTION_L 1 // left servo - pitch orientation<br>
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation)<br>
#define ROLL_DIRECTION_L 1 // left servo - roll orientation<br>
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)<br>
#define WING_LEFT_MID 1500 // left servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
#define WING_RIGHT_MID 1500 // right servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000]<br>
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000]<br>
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]<br>
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]<br>
<br>
/*********************** Airplane ***********************/<br>
#define SERVO_RATES {100, 100, 100, 100, 100, 100, 100, 100} // Rates in 0-100%<br>
#define SERVO_DIRECTION { -1, 1, 1, -1, 1, 1, 1, 1 } // Invert servos by setting -1<br>
<br>
//#define FLAPPERONS AUX4 // Mix Flaps with Aileroins.<br>
#define FLAPPERON_EP { 1500, 1700 } // Endpooints for flaps on a 2 way switch else set {1020,2000} and program in radio.<br>
//#define FLAPPERON_EP { 1200, 1500 } // Or Flapperons up for CrowMix <br>
#define FLAPPERON_INVERT { 1, -1 } // Change direction om flapperons { Wing1, Wing2 }<br>
<br>
//#define FLAPS AUX4 // Traditional Flaps on A2 invert with SERVO_DIRECTION servo[2).<br>
#define FLAP_EP { 1500, 1900 } // Endpooints for flaps on a 2 way switch else set {1020,2000} and program in radio.<br>
<br>
//#define FLAPSPEED 3 // Make flaps move slowm Higher value is Higher Speed.<br>
<br>
/*********************** Common for Heli & Airplane ***********************/<br>
//#define D12_POWER // Use D12 on PROMINI to power sensors. Will disable servo[4] on D12<br>
#define SERVO_OFFSET { 0, 0, 0, 0, 0, 0, 0, 0 } // Adjust Servo MID Offset & Swash angles<br>
// Selectable channels:= ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4<br>
<br>
/*********************** Heli ***********************/<br>
/* Channel to control CollectivePitch */<br>
#define COLLECTIVE_PITCH THROTTLE<br>
/* Set Maximum available movement for the servos. Depending on model */<br>
#define SERVO_ENDPOINT_HIGH {2000,2000,2000,2000,2000,2000,2000,2000};<br>
#define SERVO_ENDPOINT_LOW {1020,1020,1020,1020,1020,1020,1020,1020};<br>
<br>
/* Limit the range of Collective Pitch. 100% is Full Range each way and position for Zero Pitch */<br>
#define COLLECTIVE_RANGE { 80, 1500, 80 }// {Min%, ZeroPitch, Max%}.<br>
#define YAW_CENTER 1500 // Use servo[5] SERVO_ENDPOINT_HIGH/LOW for the endpoits.<br>
#define YAWMOTOR 0 // If a motor is useed as YAW Set to 1 else set to 0.<br>
<br>
/* Servo mixing for heli 120 Use 1/10 fractions (ex.5 = 5/10 = 1/2)<br>
{Coll,Nick,Roll} */<br>
#define SERVO_NICK { +10, -10, -0 }<br>
#define SERVO_LEFT { +10, +5, +10 } <br>
#define SERVO_RIGHT { +10, +5, -10 } <br>
<br>
/* Servo mixing for heli 90 <br>
{Coll,Nick,Roll} */<br>
#define SERVO_DIRECTIONS { +1, -1, -1 } // -1 will invert servo<br>
<br>
/* Limit Maximum controll for Roll & Nick in 0-100% */<br>
#define CONTROLL_RANGE { 100, 100 } // { ROLL,PITCH }<br>
<br>
/* use servo code to drive the throttle output. You want this for analog servo driving the throttle on IC engines.<br>
if inactive, throttle output will be treated as a motor output, so it can drive an ESC */<br>
//#define HELI_USE_SERVO_FOR_THROTTLE<br>
<br>
/*********************** Single and DualCopter Settings ***********************/<br>
/* Change to -1 to reverse servomovement per axis<br>
Servosettings for SingleCopter */<br>
#define SINGLECOPTRER_YAW {1, 1, 1, 1} // Left, Right,Front,Rear<br>
#define SINGLECOPTRER_SERVO {1,-1, 1,-1} // Pitch,Pitch,Roll, Roll <br>
<br>
/* Servosettings for DualCopter */<br>
#define DUALCOPTER_SERVO {1,1} //Pitch,Roll<br>
/* Use SERVO_OFFSET and SERVO_RATES in Heli and Airplane section for centering and endpoints */<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 3 - RC SYSTEM SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/* note: no need to uncomment something in this section if you use a standard receiver */<br>
<br>
/**************************************************************************************/<br>
/******** special receiver types ********************/<br>
/**************************************************************************************/<br>
<br>
/**************************** PPM Sum Reciver ***********************************/<br>
/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2<br>
Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */<br>
//#define SERIAL_SUM_PPM PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4 //For Graupner/Spektrum<br>
//#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For Robe/Hitec/Futaba<br>
//#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For some Hitec/Sanwa/Others<br>
<br>
/********************** Spektrum Satellite Reciver *******************************/<br>
/* The following lines apply only for Spektrum Satellite Receiver<br>
Spektrum Satellites are 3V devices. DO NOT connect to 5V!<br>
For MEGA boards, attach sat grey wire to RX1, pin 19. Sat black wire to ground. Sat orange wire to Mega board's 3.3V (or any other 3V to 3.3V source).<br>
For PROMINI, attach sat grey to RX0. Attach sat black to ground. */<br>
//#define SPEKTRUM 1024<br>
//#define SPEKTRUM 2048<br>
<br>
/******************************* SBUS RECIVER ************************************/<br>
/* The following line apply only for Futaba S-Bus Receiver on MEGA boards at RX1 only (Serial 1).<br>
You have to invert the S-Bus-Serial Signal e.g. with a Hex-Inverter like IC SN74 LS 04 */<br>
//#define SBUS<br>
<br>
/******************* RC signal from the serial port via Multiwii Serial Protocol *********/<br>
//#define RCSERIAL<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 4 - ALTERNATE CPUs & BOARDS *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/**************************************************************************************/<br>
/******** Promini Specifig Settings ********************/<br>
/**************************************************************************************/<br>
<br>
/************************** Hexa Motor 5 & 6 Pins *******************************/<br>
/* PIN A0 and A1 instead of PIN D5 & D6 for 6 motors config and promini config<br>
This mod allow the use of a standard receiver on a pro mini<br>
(no need to use a PPM sum receiver) */<br>
//#define A0_A1_PIN_HEX<br>
<br>
/********************************* Aux 2 Pin ***********************************/<br>
/* possibility to use PIN8 or PIN12 as the AUX2 RC input (only one, not both)<br>
it deactivates in this case the POWER PIN (pin 12) or the BUZZER PIN (pin 8) */<br>
//#define RCAUXPIN8<br>
//#define RCAUXPIN12<br>
<br>
<br>
/**************************************************************************************/<br>
/***************** Teensy 2.0 Support ******************/<br>
/**************************************************************************************/<br>
/* uncomment this if you use a teensy 2.0 with teensyduino<br>
it needs to run at 16MHz */<br>
//#define TEENSY20<br>
<br>
<br>
/**************************************************************************************/<br>
/******** Settings for ProMicro, Leonardo and other Atmega32u4 Boards ***********/<br>
/**************************************************************************************/<br>
<br>
/********************************* pin Layout **********************************/<br>
/* activate this for a better pinlayout if all pins can be used => not possible on ProMicro */<br>
//#define A32U4ALLPINS<br>
<br>
/********************************** PWM Setup **********************************/<br>
/* activate all 6 hardware PWM outputs Motor 5 = D11 and 6 = D13. <br>
note: not possible on the sparkfun promicro (pin 11 & 13 are not broken out there)<br>
if activated:<br>
Motor 1-6 = 10-bit hardware PWM<br>
Motor 7-8 = 8-bit Software PWM<br>
Servos = 8-bit Software PWM<br>
if deactivated:<br>
Motor 1-4 = 10-bit hardware PWM<br>
Motor 5-8 = 10-bit Software PWM<br>
Servos = 10-bit Software PWM */<br>
//#define HWPWM6<br>
<br>
/********************************** Aux 2 Pin **********************************/<br>
/* AUX2 pin on pin RXO */<br>
//#define RCAUX2PINRXO<br>
<br>
/* aux2 pin on pin D17 (RXLED) */<br>
//#define RCAUX2PIND17<br>
<br>
/********************************** Buzzer Pin **********************************/<br>
/* this moves the Buzzer pin from TXO to D8 for use with ppm sum or spectrum sat. RX (not needed if A32U4ALLPINS is active) */<br>
//#define D8BUZZER<br>
<br>
/*********************** Promicro version related ****************************/<br>
/* Inverted status LED for Promicro ver 10 */<br>
//#define PROMICRO10<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 5 - ALTERNATE SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/****** Serial com speed *********************************/<br>
/* This is the speed of the serial interface */<br>
#define SERIAL_COM_SPEED 115200<br>
<br>
/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config<br>
if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000<br>
it is relevent only for a conf with NK */<br>
#define INTERLEAVING_DELAY 3000<br>
<br>
/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds<br>
it is relevent only for a conf with at least a WMP */<br>
#define NEUTRALIZE_DELAY 100000<br>
<br>
<br>
/**************************************************************************************/<br>
/******** Gyro filters ********************/<br>
/**************************************************************************************/<br>
<br>
/********************* Lowpass filter for some gyros ****************************/<br>
/* ITG3200 & ITG3205 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try<br>
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting.<br>
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and<br>
balancing options ran out. Uncomment only one option!<br>
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/<br>
//#define ITG3200_LPF_256HZ // This is the default setting, no need to uncomment, just for reference<br>
//#define ITG3200_LPF_188HZ<br>
//#define ITG3200_LPF_98HZ<br>
//#define ITG3200_LPF_42HZ<br>
//#define ITG3200_LPF_20HZ<br>
//#define ITG3200_LPF_10HZ // Use this only in extreme cases, rather change motors and/or props<br>
<br>
/* MPU6050 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try<br>
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting.<br>
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and<br>
balancing options ran out. Uncomment only one option!<br>
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/<br>
//#define MPU6050_LPF_256HZ // This is the default setting, no need to uncomment, just for reference<br>
//#define MPU6050_LPF_188HZ<br>
//#define MPU6050_LPF_98HZ<br>
//#define MPU6050_LPF_42HZ<br>
//#define MPU6050_LPF_20HZ<br>
//#define MPU6050_LPF_10HZ<br>
//#define MPU6050_LPF_5HZ // Use this only in extreme cases, rather change motors and/or props<br>
<br>
/****** Gyro smoothing **********************************/<br>
/* GYRO_SMOOTHING. In case you cannot reduce vibrations _and_ _after_ you have tried the low pass filter options, you<br>
may try this gyro smoothing via averaging. Not suitable for multicopters!<br>
Good results for helicopter, airplanes and flying wings (foamies) with lots of vibrations.*/<br>
//#define GYRO_SMOOTHING {20, 20, 3} // separate averaging ranges for roll, pitch, yaw<br>
<br>
/************************ Moving Average Gyros **********************************/<br>
//#define MMGYRO // Active Moving Average Function for Gyros<br>
//#define MMGYROVECTORLENGHT 10 // Lenght of Moving Average Vector<br>
/* Moving Average ServoGimbal Signal Output */<br>
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal<br>
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 6 - OPTIONAL FEATURES *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/* Pseudo-derivative conrtroller for level mode (experimental)<br>
Additional information: <a href="http://www.multiwii.com/forum/viewtopic.php?f=8&t=503" target="_blank" rel="nofollow">http://www.multiwii.com/forum/viewtopic.php?f=8&t=503</a> */<br>
//#define LEVEL_PDF<br>
<br>
<br>
/******** Failsave settings ********************/<br>
/* Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.<br>
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered<br>
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.<br>
This value is depended from your configuration, AUW and some other params.<br>
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.<br>
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.<br>
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/<br>
//#define FAILSAFE // uncomment to activate the failsafe function<br>
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example<br>
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example<br>
#define FAILSAVE_THROTTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case<br>
<br>
<br>
/***************** DFRobot LED RING *********************************/<br>
/* I2C DFRobot LED RING communication */<br>
//#define LED_RING<br>
<br>
/******************************** LED FLASHER ***********************************/<br>
//#define LED_FLASHER<br>
//#define LED_FLASHER_DDR DDRB<br>
//#define LED_FLASHER_PORT PORTB<br>
//#define LED_FLASHER_BIT PORTB4<br>
//#define LED_FLASHER_SEQUENCE ( (uint8_t) 0 )<br>
// create double flashes<br>
//#define LED_FLASHER_SEQUENCE_ARMED ( (uint8_t) (1<<0 | 1<<2) )<br>
// full illumination<br>
//#define LED_FLASHER_SEQUENCE_MAX 0xFF<br>
<br>
<br>
/******************************* Landing lights *********************************/<br>
/* Landing lights<br>
Use an output pin to control landing lights.<br>
They can be switched automatically when used in conjunction<br>
with altitude data from a sonar unit. */<br>
//#define LANDING_LIGHTS_DDR DDRC<br>
//#define LANDING_LIGHTS_PORT PORTC<br>
//#define LANDING_LIGHTS_BIT PORTC0<br>
<br>
/* altitude above ground (in cm) as reported by sonar */<br>
//#define LANDING_LIGHTS_AUTO_ALTITUDE 50<br>
<br>
<br>
/************************* INFLIGHT ACC Calibration *****************************/<br>
/* This will activate the ACC-Inflight calibration if unchecked */<br>
//#define INFLIGHT_ACC_CALIBRATION<br>
<br>
/************************** Disable WMP power pin *******************************/<br>
/* disable use of the POWER PIN<br>
(allready done if the option RCAUXPIN12 is selected) */<br>
//#define DISABLE_POWER_PIN<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** TX-related **************************/<br>
/**************************************************************************************/<br>
<br>
/* introduce a deadband around the stick center<br>
Must be greater than zero, comment if you dont want a deadband on roll, pitch and yaw */<br>
//#define DEADBAND 6<br>
<br>
/* defines the neutral zone of throttle stick during altitude hold, default setting is<br>
+/-20 uncommend and change the value below if you want to change it. */<br>
//#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 20 <br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** GPS **************************/<br>
/**************************************************************************************/<br>
<br>
/* GPS using a SERIAL port<br>
only available on MEGA boards (this might be possible on 328 based boards in the future)<br>
if enabled, define here the Arduino Serial port number and the UART speed<br>
note: only the RX PIN is used, the GPS is not configured by multiwii<br>
the GPS must be configured to output GGA and RMC NMEA sentences (which is generally the default conf for most GPS devices)<br>
at least 5Hz update rate. uncomment the first line to select the GPS serial port of the arduino */<br>
//#define GPS_SERIAL 2 // should be 2 for flyduino v2. It's the serial port number on arduino MEGA<br>
#define GPS_BAUD 115200<br>
<br>
//#define GPS_PROMINI_SERIAL 57600 // Will Autosense if GPS is connected when ardu boots<br>
<br>
/* I2C GPS device made with an independant arduino + GPS device<br>
including some navigation functions<br>
contribution from EOSBandi <a href="http://code.google.com/p/i2c-gps-nav/" target="_blank" rel="nofollow">http://code.google.com/p/i2c-gps-nav/</a> <br>
You have to use at least I2CGpsNav code r33 */<br>
#define I2C_GPS<br>
<br>
/* I2C GPS device made with an indeedent ATTiny[24]313 + GPS device and<br>
optional sonar device. <a href="https://github.com/wertarbyte/tiny-gps/" target="_blank" rel="nofollow">https://github.com/wertarbyte/tiny-gps/</a> */<br>
/* get GPS data from Tiny-GPS */<br>
//#define TINY_GPS<br>
/* get sonar data from Tiny-GPS */<br>
//#define TINY_GPS_SONAR<br>
<br>
/* indicate a valid GPS fix with at least 5 satellites by flashing the LED? */<br>
#define GPS_LED_INDICATOR<br>
<br>
/* GPS data readed from OSD -- still need some more code to work */<br>
//#define GPS_FROM_OSD<br>
<br>
//#define USE_MSP_WP //Enables the MSP_WP command, which is used by WinGUI to display and log Home and Poshold positions<br>
//Uncomment it if you are planning to use WinGUI - Will cost +208 bytes of Flash<br>
<br>
//#define DONT_RESET_HOME_AT_ARM // HOME position is reset at every arm, uncomment it to prohibit it (you can set home position with GyroCalibration)<br>
<br>
/* GPS navigation can control the heading */<br>
<br>
#define NAV_CONTROLS_HEADING true // copter faces toward the navigation point, maghold must be enabled for it<br>
#define NAV_TAIL_FIRST false // true - copter comes in with tail first <br>
#define NAV_SET_TAKEOFF_HEADING true // true - when copter arrives to home position it rotates it's head to takeoff direction<br>
<br>
<br>
/* Get your magnetic decliniation from here : <a href="http://magnetic-declination.com/" target="_blank" rel="nofollow">http://magnetic-declination.com/</a><br>
Convert the degree+minutes into decimal degree by ==> degree+minutes*(1/60)<br>
Note the sign on declination it could be negative or positive (WEST or EAST) */<br>
//#define MAG_DECLINIATION 3.96f //For Budapest Hungary.<br>
#define MAG_DECLINIATION 0.0f<br>
<br>
#define GPS_FILTERING // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency comment out to disable<br>
#define GPS_LOW_SPEED_D_FILTER // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise commnent out to disable<br>
#define GPS_WP_RADIUS 200 // if we are within this distance to a waypoint then we consider it reached (distance is in cm)<br>
#define NAV_SLEW_RATE 30 // Adds a rate control to nav output, will smoothen out nav angle spikes<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** LCD/OLED - display settings *********************/<br>
/**************************************************************************************/<br>
<br>
/* uncomment this line if you plan to use a LCD or OLED */<br>
//#define LCD_CONF<br>
<br>
/* to include setting the aux switches for AUX1 -> AUX4 via LCD */ //to review (activate[] is now 16 bit long)<br>
//#define LCD_CONF_AUX<br>
<br>
/* if program gets too large (>32k), need to exclude some functionality */<br>
/* uncomment to suppress some unwanted aux3 aux4 items in config menu (only useful if LCD_CONF_AUX is enabled) */<br>
//#define SUPPRESS_LCD_CONF_AUX34<br>
<br>
/***************************** The type of LCD **********************************/<br>
/* choice of LCD attached for configuration and telemetry, see notes below */<br>
//#define LCD_SERIAL3W // Alex' initial variant with 3 wires, using rx-pin for transmission @9600 baud fixed<br>
//#define LCD_TEXTSTAR // SERIAL LCD: Cat's Whisker LCD_TEXTSTAR Module CW-LCD-02 (Which has 4 input keys for selecting menus)<br>
//#define LCD_VT100 // SERIAL LCD: vt100 compatible terminal emulation (blueterm, putty, etc.)<br>
//#define LCD_ETPP // I2C LCD: Eagle Tree Power Panel LCD, which is i2c (not serial)<br>
//#define LCD_LCD03 // I2C LCD: LCD03, which is i2c<br>
//#define OLED_I2C_128x64 // I2C LCD: OLED <a href="http://www.multiwii.com/forum/viewtopic.php?f=7&t=1350" target="_blank" rel="nofollow">http://www.multiwii.com/forum/viewtopic.php?f=7&t=1350</a><br>
<br>
/****************************** Logo settings ***********************************/<br>
//#define SUPPRESS_OLED_I2C_128x64LOGO // suppress display of OLED logo to save memory<br>
<br>
/* style of display - AUTODETECTED via LCD_ setting - only activate to overwrite defaults */<br>
//#define DISPLAY_2LINES<br>
//#define DISPLAY_MULTILINE<br>
//#define MULTILINE_PRE 2 // multiline configMenu # pref lines<br>
//#define MULTILINE_POST 6 // multiline configMenu # post lines<br>
/******************************** Navigation ***********************************/<br>
/* keys to navigate the LCD menu (preset to LCD_TEXTSTAR key-depress codes)*/<br>
#define LCD_MENU_PREV 'a'<br>
#define LCD_MENU_NEXT 'c'<br>
#define LCD_VALUE_UP 'd'<br>
#define LCD_VALUE_DOWN 'b'<br>
<br>
#define LCD_MENU_SAVE_EXIT 's'<br>
#define LCD_MENU_ABORT 'x'<br>
<br>
/* To use an LCD03 for configuration:<br>
<a href="http://www.robot-electronics.co.uk/htm/Lcd03tech.htm" target="_blank" rel="nofollow">http://www.robot-electronics.co.uk/htm/Lcd03tech.htm</a><br>
Remove the jumper on its back to set i2c control.<br>
VCC to +5V VCC (pin1 from top)<br>
SDA - Pin A4 Mini Pro - Pin 20 Mega (pin2 from top)<br>
SCL - Pin A5 Mini Pro - Pin 21 Mega (pin3 from top)<br>
GND to Ground (pin4 from top)*/<br>
<br>
/* To use an Eagle Tree Power Panel LCD for configuration:<br>
White wire to Ground<br>
Red wire to +5V VCC (or to the WMP power pin, if you prefer to reset everything on the bus when WMP resets)<br>
Yellow wire to SDA - Pin A4 Mini Pro - Pin 20 Mega<br>
Brown wire to SCL - Pin A5 Mini Pro - Pin 21 Mega */<br>
<br>
/* Cat's whisker LCD_TEXTSTAR LCD<br>
Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD )<br>
Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode<br>
NO rx / tx line reconfiguration, use natural pins.<br>
The four buttons sending 'A', 'B', 'C', 'D' are supported for configuration navigation and request of telemetry pages 1-4 */<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** telemetry **************************/<br>
/**************************************************************************************/<br>
<br>
/* to monitor system values (battery level, loop time etc. with LCD enable this<br>
note: for now you must send single characters to request different pages<br>
Buttons toggle request for page on/off<br>
The active page on the LCD does get updated automatically<br>
Easy to use with Terminal application or display like LCD - if available uses the 4 preconfigured buttons to send 'A', 'B', 'C', 'D' */<br>
/******************************** Activation ***********************************/<br>
//#define LCD_TELEMETRY<br>
<br>
/* to enable automatic hopping between a choice of telemetry pages uncomment this.<br>
This may be useful if your LCD has no buttons or the sending is broken<br>
hopping is activated and deactivated in unarmed mode with throttle=low & roll=left & pitch=forward<br>
set it to the sequence of telemetry pages you want to see<br>
2 line displays support pages 1-9<br>
multiline displays support pages 1-5 */<br>
//#define LCD_TELEMETRY_AUTO "123452679" // pages 1 to 7 in ascending order<br>
//#define LCD_TELEMETRY_AUTO "212232425262729" // strong emphasis on page 2<br>
<br>
/* same as above, but manual stepping sequence; requires <br>
stick input (throttle=low & roll=right & pitch=forward) to <br>
step through each defined telemetry page */<br>
//#define LCD_TELEMETRY_STEP "0123456789" // must begin with 0<br>
<br>
<br>
/* on telemetry page B (2) it gives a bar graph which shows how much voltage battery has left. Range from 0 to 12 Volt is not very informative<br>
so we try do define a meaningful range. For a 3S battery we define full=12,6V and calculate how much it is above first warning level<br>
Example: 12.6V - VBATLEVEL1_3S (for me = 126 - 102 = 24) */<br>
#define VBATREF 24<br>
<br>
/* if program gets too large (>32k), need to exclude some functionality<br>
uncomment to suppress some unwanted telemetry pages (only useful if telemetry is enabled) */<br>
//#define SUPPRESS_TELEMETRY_PAGE_1<br>
//#define SUPPRESS_TELEMETRY_PAGE_2<br>
//#define SUPPRESS_TELEMETRY_PAGE_3<br>
//#define SUPPRESS_TELEMETRY_PAGE_4<br>
//#define SUPPRESS_TELEMETRY_PAGE_5<br>
//#define SUPPRESS_TELEMETRY_PAGE_6<br>
//#define SUPPRESS_TELEMETRY_PAGE_7<br>
//#define SUPPRESS_TELEMETRY_PAGE_8<br>
//#define SUPPRESS_TELEMETRY_PAGE_9<br>
<br>
/********************************************************************/<br>
/**** Buzzer ****/<br>
/********************************************************************/<br>
//#define BUZZER<br>
//#define RCOPTIONSBEEP //uncomment this if you want the buzzer to beep at any rcOptions change on channel Aux1 to Aux4<br>
//#define ARMEDTIMEWARNING 330 // Trigger an alarm after a certain time of being armed [s] to save you lipo (if your TX does not have a countdown)<br>
<br>
/********************************************************************/<br>
/**** battery voltage monitoring ****/<br>
/********************************************************************/<br>
/* for V BAT monitoring<br>
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN<br>
with R1=33k and R2=51k<br>
vbat = [0;1023]*16/VBATSCALE<br>
must be associated with #define BUZZER ! */<br>
//#define VBAT // uncomment this line to activate the vbat code<br>
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage<br>
#define VBATLEVEL1_3S 107 // 10,7V<br>
#define VBATLEVEL2_3S 103 // 10,3V<br>
#define VBATLEVEL3_3S 99 // 9.9V<br>
#define NO_VBAT 16 // Avoid beeping without any battery<br>
<br>
<br>
/********************************************************************/<br>
/**** powermeter (battery capacity monitoring) ****/<br>
/********************************************************************/<br>
<br>
/* enable monitoring of the power consumption from battery (think of mAh)<br>
allows to set alarm value in GUI or via LCD<br>
Two options:<br>
1 - soft: - (good results +-5% for plush and mystery ESCs @ 2S and 3S, not good with SuperSimple ESC<br>
00. relies on your combo of battery type (Voltage, cpacity), ESC, ESC settings, motors, props and multiwii cycle time<br>
01. set POWERMETER soft. Uses PLEVELSCALE = 50, PLEVELDIV = PLEVELDIVSOFT = 5000<br>
0. output is a value that linearily scales to power (mAh)<br>
1. get voltage reading right first<br>
2. start with freshly charged battery<br>
3. go fly your typical flight (routine and duration)<br>
4. at end connect to GUI or LCD and read the power value; write it down (example 4711)<br>
5. charge battery, write down amount of energy needed (example 722 mAh)<br>
6. compute alarm value for desired power threshold (example 750 mAh : alarm = 4711 / 722 * 750)<br>
7. set alarm value in GUI or LCD<br>
8. enjoy your new battery alarm - possibly repeat steps 2 .. 7<br>
9. if you want the numbers to represent your mAh value, you must change PLEVELDIV<br>
2 - hard: - (uses hardware sensor, after configuration gives reasonable results<br>
00. uses analog pin 2 to read voltage output from sensor.<br>
01. set POWERMETER hard. Uses PLEVELSCALE = 50<br>
02. install low path filter for 25 Hz to sensor input<br>
03. check your average cycle time. If not close to 3ms, then you must change PLEVELDIV accordingly<br>
1. compute PLEVELDIV for your sensor (see below for insturctions)<br>
2. set PLEVELDIVSOFT to 5000 ( to use LOG_VALUES for individual motor comparison)<br>
3. attach, set PSENSORNULL and PINT2mA<br>
4. configure, compile, upload, set alarm value in GUI or LCD<br>
3. enjoy true readings of mAh consumed<br>
set POWERMETER to "soft" (1) or "hard" (2) depending on sensor you want to utilize */<br>
//#define POWERMETER_SOFT<br>
//#define POWERMETER_HARD<br>
/* the sum of all powermeters ranges from [0:60000 e4] theoretically.<br>
the alarm level from eeprom is out of [0:255], so we multipy alarm level with PLEVELSCALE and with 1e4 before comparing<br>
PLEVELSCALE is the step size you can use to set alarm */<br>
#define PLEVELSCALE 50 // if you change this value for other granularity, you must search for comments in code to change accordingly<br>
/* larger PLEVELDIV will get you smaller value for power (mAh equivalent) */<br>
#define PLEVELDIV 5000 // default for soft - if you lower PLEVELDIV, beware of overrun in uint32 pMeter<br>
#define PLEVELDIVSOFT PLEVELDIV // for soft always equal to PLEVELDIV; for hard set to 5000<br>
//#define PLEVELDIV 1361L // to convert the sum into mAh divide by this value<br>
/* amploc 25A sensor has 37mV/A<br>
arduino analog resolution is 4.9mV per unit; units from [0..1023]<br>
sampling rate 20ms, approx 19977 micro seconds<br>
PLEVELDIV = 37 / 4.9 * 10e6 / 18000 * 3600 / 1000 = 1361L<br>
set to analogRead() value for zero current */<br>
#define PSENSORNULL 510 // for I=0A my sensor gives 1/2 Vss; that is approx 2.49Volt<br>
#define PINT2mA 13 // for telemtry display: one integer step on arduino analog translates to mA (example 4.9 / 37 * 100<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 7 - TUNING & DEVELOPER **************/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/**************************************************************************************/<br>
/******** special ESC with extended range [0-2000] microseconds ********************/<br>
/**************************************************************************************/<br>
//#define EXT_MOTOR_RANGE<br>
<br>
/**************************************************************************************/<br>
/*********************** motor, servo and other presets ***********************/<br>
/**************************************************************************************/<br>
/* motors will not spin when the throttle command is in low position<br>
this is an alternative method to stop immediately the motors */<br>
//#define MOTOR_STOP<br>
<br>
/* some radios have not a neutral point centered on 1500. can be changed here */<br>
#define MIDRC 1500<br>
<br>
/*********************** Servo Refreshrates ***********************/<br>
/* Default 50Hz Servo refresh rate*/<br>
#define SERVO_RFR_50HZ<br>
<br>
/* up to 160Hz servo refreshrate .. works with the most analog servos*/<br>
//#define SERVO_RFR_160HZ<br>
<br>
/* up to 300Hz refreshrate it is as fast as possible (100-300Hz depending on the cound of used servos and the servos state).<br>
for use with digital servos<br>
dont use it with analog servos! thay may get damage. (some will work but be careful) */<br>
//#define SERVO_RFR_300HZ<br>
<br>
/*********************** HW PWM Servos ***********************/ <br>
/* HW PWM Gimbal for Arduino Mega.. moves:<br>
Pitch = pin 44<br>
Roll = pin 45<br>
this reduces the PWM resolution for all other servos to 8 bit */<br>
//#define MEGA_HW_GIMBAL<br>
<br>
/********************************************************************/<br>
/**** IMU complimentary filter tuning ****/<br>
/********************************************************************/<br>
<br>
/* Set the Low Pass Filter factor for ACC<br>
Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define ACC_LPF_FACTOR 100<br>
<br>
/* Set the Low Pass Filter factor for Magnetometer<br>
Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define MG_LPF_FACTOR 4<br>
<br>
/* Set the Gyro Weight for Gyro/Acc complementary filter<br>
Increasing this value would reduce and delay Acc influence on the output of the filter<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define GYR_CMPF_FACTOR 400.0f<br>
<br>
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter<br>
Increasing this value would reduce and delay Magnetometer influence on the output of the filter<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define GYR_CMPFM_FACTOR 200.0f<br>
<br>
<br>
/********************************************************************/<br>
/**** diagnostics ****/<br>
/********************************************************************/<br>
<br>
/* to log values like max loop time and others to come<br>
logging values are visible via LCD config<br>
set to 2, if you want powerconsumption on a per motor basis (this uses the big array and is a memory hog, if POWERMETER <> PM_SOFT) */<br>
//#define LOG_VALUES 1<br>
<br>
/* to add debugging code<br>
not needed and not recommended for normal operation<br>
will add extra code that may slow down the main loop or make copter non-flyable */<br>
//#define DEBUG<br>
<br>
/* Use this to trigger LCD configuration without a TX - only for debugging - do NOT fly with this activated */<br>
//#define LCD_CONF_DEBUG<br>
<br>
/* Use this to trigger telemetry without a TX - only for debugging - do NOT fly with this activated */<br>
//#define LCD_TELEMETRY_DEBUG //This form rolls between all screens, LCD_TELEMETRY_AUTO must also be defined.<br>
//#define LCD_TELEMETRY_DEBUG 6 //This form stays on the screen specified.<br>
<br>
<br>
/********************************************************************/<br>
/**** ESCs calibration ****/<br>
/********************************************************************/<br>
<br>
/* to calibrate all ESCs connected to MWii at the same time (useful to avoid unplugging/re-plugging each ESC)<br>
Warning: this creates a special version of MultiWii Code<br>
You cannot fly with this special version. It is only to be used for calibrating ESCs<br>
Read How To at <a href="http://code.google.com/p/multiwii/wiki/ESCsCalibration" target="_blank" rel="nofollow">http://code.google.com/p/multiwii/wiki/ESCsCalibration</a> */<br>
#define ESC_CALIB_LOW MINCOMMAND<br>
#define ESC_CALIB_HIGH 2000<br>
//#define ESC_CALIB_CANNOT_FLY // uncomment to activate<br>
<br>
/**** internal frequencies ****/<br>
/* frequenies for rare cyclic actions in the main loop, depend on cycle time<br>
time base is main loop cycle time - a value of 6 means to trigger the action every 6th run through the main loop<br>
example: with cycle time of approx 3ms, do action every 6*3ms=18ms<br>
value must be [1; 65535] */<br>
#define LCD_TELEMETRY_FREQ 23 // to send telemetry data over serial 23 <=> 60ms <=> 16Hz (only sending interlaced, so 8Hz update rate)<br>
#define LCD_TELEMETRY_AUTO_FREQ 667 // to step to next telemetry page 667 <=> 2s<br>
#define PSENSORFREQ 6 // to read hardware powermeter sensor 6 <=> 18ms<br>
#define VBATFREQ PSENSORFREQ // to read battery voltage - keep equal to PSENSORFREQ unless you know what you are doing<br>
<br>
/********************************************************************/<br>
/**** Regression testing ****/<br>
/********************************************************************/<br>
<br>
/* for development only:<br>
to allow for easier and reproducable config sets for test compiling, different sets of config parameters are kept<br>
together. This is meant to help detecting compile time errors for various features in a coordinated way.<br>
It is not meant to produce your flying firmware<br>
To use:<br>
- do not set any options in config.h,<br>
- enable with #define COPTERTEST 1, then compile<br>
- if possible, check for the size<br>
- repeat with other values of 2, 3, 4 etc.<br>
*/<br>
//#define COPTERTEST 1<br>
<br>
/*************************************************************************************************/<br>
/**** END OF CONFIGURABLE PARAMETERS ****/<br>
/*************************************************************************************************/<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/**** CONFIGURABLE PARAMETERS ****/<br>
/*************************************************************************************************/<br>
<br>
/* this file consists of several sections<br>
* to create a working combination you must at least make your choices in section 1.<br>
* 1 - BASIC SETUP - you must select an option in every block.<br>
* this assumes you have 4 channels connected to your board with standard ESCs and servos.<br>
* 2 - COPTER TYPE SPECIFIC OPTIONS - you likely want to check for options for your copter type<br>
* 3 - RC SYSTEM SETUP<br>
* 4 - ALTERNATE CPUs & BOARDS - if you have<br>
* 5 - ALTERNATE SETUP - select alternate RX (SBUS, PPM, etc.), alternate ESC-range, etc. here<br>
* 6 - OPTIONAL FEATURES - enable nice to have features here (LCD, telemetry, battery monitor etc.)<br>
* 7 - TUNING & DEVELOPER - if you know what you are doing; you have been warned<br>
*/<br>
<br>
<br>
MULTIWII 2.1 (config.h)<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 1 - BASIC SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/************************** The type of multicopter ****************************/<br>
//#define GIMBAL<br>
//#define BI<br>
//#define TRI<br>
//#define QUADP<br>
#define QUADX<br>
//#define Y4<br>
//#define Y6<br>
//#define HEX6<br>
//#define HEX6X<br>
//#define OCTOX8<br>
//#define OCTOFLATP<br>
//#define OCTOFLATX<br>
//#define FLYING_WING<br>
//#define VTAIL4<br>
//#define AIRPLANE<br>
//#define SINGLECOPTER<br>
//#define DUALCOPTER<br>
//#define HELI_120_CCPM<br>
//#define HELI_90_DEG<br>
<br>
/**************************** Motor minthrottle *******************************/<br>
/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)<br>
This is the minimum value that allow motors to run at a idle speed */<br>
#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A<br>
//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A<br>
//#define MINTHROTTLE 1064 // special ESC (simonk)<br>
//#define MINTHROTTLE 1150<br>
<br>
/**************************** Motor maxthrottle *******************************/<br>
/* this is the maximum value for the ESCs at full power, this value can be increased up to 2000 */<br>
#define MAXTHROTTLE 1850<br>
<br>
/**************************** Mincommand *******************************/<br>
/* this is the value for the ESCs when they are not armed<br>
in some cases, this value must be lowered down to 900 for some specific ESCs, otherwise they failed to initiate */<br>
#define MINCOMMAND 1000<br>
<br>
/********************************** I2C speed ************************************/<br>
#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP<br>
//#define I2C_SPEED 400000L //400kHz fast mode, it works only with some WMP clones<br>
<br>
/*************************** Internal i2c Pullups ********************************/<br>
/* enable internal I2C pull ups (in most cases it is better to use external pullups) */<br>
//#define INTERNAL_I2C_PULLUPS<br>
<br>
/**************************************************************************************/<br>
/***************** boards and sensor definitions ******************/<br>
/**************************************************************************************/<br>
<br>
/*************************** Combined IMU Boards ********************************/<br>
/* if you use a specific sensor board:<br>
please submit any correction to this list.<br>
Note from Alex: I only own some boards, for other boards, I'm not sure, the info was gathered via rc forums, be cautious */<br>
//#define FFIMUv1 // first 9DOF+baro board from Jussi, with HMC5843 <- confirmed by Alex<br>
//#define FFIMUv2 // second version of 9DOF+baro board from Jussi, with HMC5883 <- confirmed by Alex<br>
//#define FREEIMUv1 // v0.1 & v0.2 & v0.3 version of 9DOF board from Fabio<br>
//#define FREEIMUv03 // FreeIMU v0.3 and v0.3.1<br>
//#define FREEIMUv035 // FreeIMU v0.3.5 no baro<br>
//#define FREEIMUv035_MS // FreeIMU v0.3.5_MS <- confirmed by Alex<br>
//#define FREEIMUv035_BMP // FreeIMU v0.3.5_BMP<br>
//#define FREEIMUv04 // FreeIMU v0.4 with MPU6050, HMC5883L, MS561101BA <- confirmed by Alex<br>
//#define FREEIMUv043 // same as FREEIMUv04 with final MPU6050 (with the right ACC scale)<br>
//#define NANOWII // the smallest multiwii FC based on MPU6050 + pro micro based proc <- confirmed by Alex<br>
//#define PIPO // 9DOF board from erazz<br>
//#define QUADRINO // full FC board 9DOF+baro board from witespy with BMP085 baro <- confirmed by Alex<br>
//#define QUADRINO_ZOOM // full FC board 9DOF+baro board from witespy second edition<br>
//#define QUADRINO_ZOOM_MS// full FC board 9DOF+baro board from witespy second edition <- confirmed by Alex<br>
//#define ALLINONE // full FC board or standalone 9DOF+baro board from CSG_EU<br>
//#define AEROQUADSHIELDv2<br>
//#define ATAVRSBIN1 // Atmel 9DOF (Contribution by EOSBandi). requires 3.3V power.<br>
//#define SIRIUS // Sirius Navigator IMU <- confirmed by Alex<br>
//#define SIRIUS600 // Sirius Navigator IMU using the WMP for the gyro<br>
//#define MINIWII // Jussi's MiniWii Flight Controller <- confirmed by Alex<br>
//#define CITRUSv2_1 // CITRUS from qcrc.ca<br>
//#define CHERRY6DOFv1_0<br>
//#define DROTEK_10DOF // Drotek 10DOF with ITG3200, BMA180, HMC5883, BMP085, w or w/o LLC<br>
//#define DROTEK_10DOF_MS // Drotek 10DOF with ITG3200, BMA180, HMC5883, MS5611, LLC<br>
//#define DROTEK_6DOFv2 // Drotek 6DOF v2<br>
//#define DROTEK_6DOF_MPU // Drotek 6DOF with MPU6050<br>
//#define DROTEK_10DOF_MPU//<br>
//#define MONGOOSE1_0 // mongoose 1.0 <a href="http://store.ckdevices.com/" target="_blank" rel="nofollow">http://store.ckdevices.com/</a><br>
//#define CRIUS_LITE // Crius MultiWii Lite<br>
#define CRIUS_SE // Crius MultiWii SE<br>
//#define OPENLRSv2MULTI // OpenLRS v2 Multi Rc Receiver board including ITG3205 and ADXL345<br>
//#define BOARD_PROTO_1 // with MPU6050 + HMC5883L + MS baro<br>
//#define BOARD_PROTO_2 // with MPU6050 + slave MAG3110 + MS baro<br>
//#define GY_80 // Chinese 10 DOF with L3G4200D ADXL345 HMC5883L BMP085, LLC<br>
//#define GY_85 // Chinese 9 DOF with ITG3205 ADXL345 HMC5883L LLC<br>
//#define GY_86 // Chinese 10 DOF with MPU6050 HMC5883L MS5611, LLC<br>
//#define INNOVWORKS_10DOF // with ITG3200, BMA180, HMC5883, BMP085 available here <a href="http://www.diymulticopter.com" target="_blank" rel="nofollow">http://www.diymulticopter.com</a><br>
//#define INNOVWORKS_6DOF // with ITG3200, BMA180 available here <a href="http://www.diymulticopter.com" target="_blank" rel="nofollow">http://www.diymulticopter.com</a><br>
//#define PROTO_DIY // 10DOF mega board<br>
//#define IOI_MINI_MULTIWII// <a href="http://www.bambucopter.com" target="_blank" rel="nofollow">www.bambucopter.com</a><br>
//#define Bobs_6DOF_V1 // BobsQuads 6DOF V1 with ITG3200 & BMA180<br>
//#define Bobs_9DOF_V1 // BobsQuads 9DOF V1 with ITG3200, BMA180 & HMC5883L<br>
//#define Bobs_10DOF_BMP_V1 // BobsQuads 10DOF V1 with ITG3200, BMA180, HMC5883L & BMP180 - BMP180 is software compatible with BMP085<br>
//#define FLYDUINO_MPU<br>
//#define CRIUS_AIO_PRO_V1<br>
<br>
/*************************** independent sensors ********************************/<br>
/* leave it commented if you already checked a specific board above */<br>
/* I2C gyroscope */<br>
//#define WMP<br>
#define ITG3200<br>
//#define L3G4200D<br>
//#define MPU6050 //combo + ACC<br>
<br>
/* I2C accelerometer */<br>
//#define NUNCHUCK // if you want to use the nunckuk connected to a WMP<br>
//#define MMA7455<br>
//#define ADXL345<br>
//#define BMA020<br>
#define BMA180<br>
//#define NUNCHACK // if you want to use the nunckuk as a standalone I2C ACC without WMP<br>
//#define LIS3LV02<br>
//#define LSM303DLx_ACC<br>
<br>
/* I2C barometer */<br>
#define BMP085<br>
//#define MS561101BA<br>
<br>
/* I2C magnetometer */<br>
//#define HMC5843<br>
#define HMC5883<br>
//#define AK8975<br>
//#define MAG3110<br>
<br>
/* Sonar */ // for visualization purpose currently - no control code behind<br>
//#define SRF02 // use the Devantech SRF i2c sensors<br>
//#define SRF08<br>
//#define SRF10<br>
//#define SRF23<br>
<br>
/* ADC accelerometer */ // for 5DOF from sparkfun, uses analog PIN A1/A2/A3<br>
//#define ADCACC<br>
<br>
/* individual sensor orientation */<br>
//#define ACC_ORIENTATION(X, Y, Z) {accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z;}<br>
//#define GYRO_ORIENTATION(X, Y, Z) {gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z;}<br>
//#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z;}<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 2 - COPTER TYPE SPECIFIC OPTIONS *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/******************************** TRI *********************************/<br>
#define YAW_DIRECTION 1<br>
//#define YAW_DIRECTION -1 // if you want to reverse the yaw correction direction<br>
/* you can change the tricopter servo travel here */<br>
#define TRI_YAW_CONSTRAINT_MIN 1020<br>
#define TRI_YAW_CONSTRAINT_MAX 2000<br>
#define TRI_YAW_MIDDLE 1500 // tail servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
<br>
/******************************** ARM/DISARM *********************************/<br>
/* optionally disable stick combinations to arm/disarm the motors.<br>
* In most cases one of the two options to arm/disarm via TX stick is sufficient */<br>
#define ALLOW_ARM_DISARM_VIA_TX_YAW<br>
//#define ALLOW_ARM_DISARM_VIA_TX_ROLL<br>
<br>
/*********************** Cam Stabilisation ***********************/<br>
/* The following lines apply only for a pitch/roll tilt stabilization system. Uncomment the first or second line to activate it */<br>
//#define SERVO_MIX_TILT<br>
//#define SERVO_TILT<br>
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020<br>
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000<br>
#define TILT_PITCH_MIDDLE 1500 //servo neutral value<br>
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement<br>
#define TILT_ROLL_MIN 1020<br>
#define TILT_ROLL_MAX 2000<br>
#define TILT_ROLL_MIDDLE 1500<br>
#define TILT_ROLL_PROP 10<br>
<br>
/* camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */<br>
//#define CAMTRIG<br>
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo<br>
#define CAM_SERVO_LOW 1020 // the position of LOW state servo<br>
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms<br>
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms<br>
<br>
/*********************** Flying Wing ***********************/<br>
/* you can change change servo orientation and servo min/max values here<br>
valid for all flight modes, even passThrough mode<br>
need to setup servo directions here; no need to swap servos amongst channels at rx */<br>
#define PITCH_DIRECTION_L 1 // left servo - pitch orientation<br>
#define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation)<br>
#define ROLL_DIRECTION_L 1 // left servo - roll orientation<br>
#define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation)<br>
#define WING_LEFT_MID 1500 // left servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
#define WING_RIGHT_MID 1500 // right servo center pos. - use this for initial trim; later trim midpoint via LCD<br>
#define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000]<br>
#define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000]<br>
#define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000]<br>
#define WING_RIGHT_MAX 2000 // limit servo travel range must be inside [1020;2000]<br>
<br>
/*********************** Airplane ***********************/<br>
#define SERVO_RATES {100, 100, 100, 100, 100, 100, 100, 100} // Rates in 0-100%<br>
#define SERVO_DIRECTION { -1, 1, 1, -1, 1, 1, 1, 1 } // Invert servos by setting -1<br>
<br>
//#define FLAPPERONS AUX4 // Mix Flaps with Aileroins.<br>
#define FLAPPERON_EP { 1500, 1700 } // Endpooints for flaps on a 2 way switch else set {1020,2000} and program in radio.<br>
//#define FLAPPERON_EP { 1200, 1500 } // Or Flapperons up for CrowMix <br>
#define FLAPPERON_INVERT { 1, -1 } // Change direction om flapperons { Wing1, Wing2 }<br>
<br>
//#define FLAPS AUX4 // Traditional Flaps on A2 invert with SERVO_DIRECTION servo[2).<br>
#define FLAP_EP { 1500, 1900 } // Endpooints for flaps on a 2 way switch else set {1020,2000} and program in radio.<br>
<br>
//#define FLAPSPEED 3 // Make flaps move slowm Higher value is Higher Speed.<br>
<br>
/*********************** Common for Heli & Airplane ***********************/<br>
//#define D12_POWER // Use D12 on PROMINI to power sensors. Will disable servo[4] on D12<br>
#define SERVO_OFFSET { 0, 0, 0, 0, 0, 0, 0, 0 } // Adjust Servo MID Offset & Swash angles<br>
// Selectable channels:= ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4<br>
<br>
/*********************** Heli ***********************/<br>
/* Channel to control CollectivePitch */<br>
#define COLLECTIVE_PITCH THROTTLE<br>
/* Set Maximum available movement for the servos. Depending on model */<br>
#define SERVO_ENDPOINT_HIGH {2000,2000,2000,2000,2000,2000,2000,2000};<br>
#define SERVO_ENDPOINT_LOW {1020,1020,1020,1020,1020,1020,1020,1020};<br>
<br>
/* Limit the range of Collective Pitch. 100% is Full Range each way and position for Zero Pitch */<br>
#define COLLECTIVE_RANGE { 80, 1500, 80 }// {Min%, ZeroPitch, Max%}.<br>
#define YAW_CENTER 1500 // Use servo[5] SERVO_ENDPOINT_HIGH/LOW for the endpoits.<br>
#define YAWMOTOR 0 // If a motor is useed as YAW Set to 1 else set to 0.<br>
<br>
/* Servo mixing for heli 120 Use 1/10 fractions (ex.5 = 5/10 = 1/2)<br>
{Coll,Nick,Roll} */<br>
#define SERVO_NICK { +10, -10, -0 }<br>
#define SERVO_LEFT { +10, +5, +10 } <br>
#define SERVO_RIGHT { +10, +5, -10 } <br>
<br>
/* Servo mixing for heli 90 <br>
{Coll,Nick,Roll} */<br>
#define SERVO_DIRECTIONS { +1, -1, -1 } // -1 will invert servo<br>
<br>
/* Limit Maximum controll for Roll & Nick in 0-100% */<br>
#define CONTROLL_RANGE { 100, 100 } // { ROLL,PITCH }<br>
<br>
/* use servo code to drive the throttle output. You want this for analog servo driving the throttle on IC engines.<br>
if inactive, throttle output will be treated as a motor output, so it can drive an ESC */<br>
//#define HELI_USE_SERVO_FOR_THROTTLE<br>
<br>
/*********************** Single and DualCopter Settings ***********************/<br>
/* Change to -1 to reverse servomovement per axis<br>
Servosettings for SingleCopter */<br>
#define SINGLECOPTRER_YAW {1, 1, 1, 1} // Left, Right,Front,Rear<br>
#define SINGLECOPTRER_SERVO {1,-1, 1,-1} // Pitch,Pitch,Roll, Roll <br>
<br>
/* Servosettings for DualCopter */<br>
#define DUALCOPTER_SERVO {1,1} //Pitch,Roll<br>
/* Use SERVO_OFFSET and SERVO_RATES in Heli and Airplane section for centering and endpoints */<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 3 - RC SYSTEM SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/* note: no need to uncomment something in this section if you use a standard receiver */<br>
<br>
/**************************************************************************************/<br>
/******** special receiver types ********************/<br>
/**************************************************************************************/<br>
<br>
/**************************** PPM Sum Reciver ***********************************/<br>
/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2<br>
Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */<br>
//#define SERIAL_SUM_PPM PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,AUX3,AUX4 //For Graupner/Spektrum<br>
//#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For Robe/Hitec/Futaba<br>
//#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,AUX3,AUX4 //For some Hitec/Sanwa/Others<br>
<br>
/********************** Spektrum Satellite Reciver *******************************/<br>
/* The following lines apply only for Spektrum Satellite Receiver<br>
Spektrum Satellites are 3V devices. DO NOT connect to 5V!<br>
For MEGA boards, attach sat grey wire to RX1, pin 19. Sat black wire to ground. Sat orange wire to Mega board's 3.3V (or any other 3V to 3.3V source).<br>
For PROMINI, attach sat grey to RX0. Attach sat black to ground. */<br>
//#define SPEKTRUM 1024<br>
//#define SPEKTRUM 2048<br>
<br>
/******************************* SBUS RECIVER ************************************/<br>
/* The following line apply only for Futaba S-Bus Receiver on MEGA boards at RX1 only (Serial 1).<br>
You have to invert the S-Bus-Serial Signal e.g. with a Hex-Inverter like IC SN74 LS 04 */<br>
//#define SBUS<br>
<br>
/******************* RC signal from the serial port via Multiwii Serial Protocol *********/<br>
//#define RCSERIAL<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 4 - ALTERNATE CPUs & BOARDS *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/**************************************************************************************/<br>
/******** Promini Specifig Settings ********************/<br>
/**************************************************************************************/<br>
<br>
/************************** Hexa Motor 5 & 6 Pins *******************************/<br>
/* PIN A0 and A1 instead of PIN D5 & D6 for 6 motors config and promini config<br>
This mod allow the use of a standard receiver on a pro mini<br>
(no need to use a PPM sum receiver) */<br>
//#define A0_A1_PIN_HEX<br>
<br>
/********************************* Aux 2 Pin ***********************************/<br>
/* possibility to use PIN8 or PIN12 as the AUX2 RC input (only one, not both)<br>
it deactivates in this case the POWER PIN (pin 12) or the BUZZER PIN (pin 8) */<br>
//#define RCAUXPIN8<br>
//#define RCAUXPIN12<br>
<br>
<br>
/**************************************************************************************/<br>
/***************** Teensy 2.0 Support ******************/<br>
/**************************************************************************************/<br>
/* uncomment this if you use a teensy 2.0 with teensyduino<br>
it needs to run at 16MHz */<br>
//#define TEENSY20<br>
<br>
<br>
/**************************************************************************************/<br>
/******** Settings for ProMicro, Leonardo and other Atmega32u4 Boards ***********/<br>
/**************************************************************************************/<br>
<br>
/********************************* pin Layout **********************************/<br>
/* activate this for a better pinlayout if all pins can be used => not possible on ProMicro */<br>
//#define A32U4ALLPINS<br>
<br>
/********************************** PWM Setup **********************************/<br>
/* activate all 6 hardware PWM outputs Motor 5 = D11 and 6 = D13. <br>
note: not possible on the sparkfun promicro (pin 11 & 13 are not broken out there)<br>
if activated:<br>
Motor 1-6 = 10-bit hardware PWM<br>
Motor 7-8 = 8-bit Software PWM<br>
Servos = 8-bit Software PWM<br>
if deactivated:<br>
Motor 1-4 = 10-bit hardware PWM<br>
Motor 5-8 = 10-bit Software PWM<br>
Servos = 10-bit Software PWM */<br>
//#define HWPWM6<br>
<br>
/********************************** Aux 2 Pin **********************************/<br>
/* AUX2 pin on pin RXO */<br>
//#define RCAUX2PINRXO<br>
<br>
/* aux2 pin on pin D17 (RXLED) */<br>
//#define RCAUX2PIND17<br>
<br>
/********************************** Buzzer Pin **********************************/<br>
/* this moves the Buzzer pin from TXO to D8 for use with ppm sum or spectrum sat. RX (not needed if A32U4ALLPINS is active) */<br>
//#define D8BUZZER<br>
<br>
/*********************** Promicro version related ****************************/<br>
/* Inverted status LED for Promicro ver 10 */<br>
//#define PROMICRO10<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 5 - ALTERNATE SETUP *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/****** Serial com speed *********************************/<br>
/* This is the speed of the serial interface */<br>
#define SERIAL_COM_SPEED 115200<br>
<br>
/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config<br>
if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000<br>
it is relevent only for a conf with NK */<br>
#define INTERLEAVING_DELAY 3000<br>
<br>
/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds<br>
it is relevent only for a conf with at least a WMP */<br>
#define NEUTRALIZE_DELAY 100000<br>
<br>
<br>
/**************************************************************************************/<br>
/******** Gyro filters ********************/<br>
/**************************************************************************************/<br>
<br>
/********************* Lowpass filter for some gyros ****************************/<br>
/* ITG3200 & ITG3205 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try<br>
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting.<br>
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and<br>
balancing options ran out. Uncomment only one option!<br>
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/<br>
//#define ITG3200_LPF_256HZ // This is the default setting, no need to uncomment, just for reference<br>
//#define ITG3200_LPF_188HZ<br>
//#define ITG3200_LPF_98HZ<br>
//#define ITG3200_LPF_42HZ<br>
//#define ITG3200_LPF_20HZ<br>
//#define ITG3200_LPF_10HZ // Use this only in extreme cases, rather change motors and/or props<br>
<br>
/* MPU6050 Low pass filter setting. In case you cannot eliminate all vibrations to the Gyro, you can try<br>
to decrease the LPF frequency, only one step per try. As soon as twitching gone, stick with that setting.<br>
It will not help on feedback wobbles, so change only when copter is randomly twiching and all dampening and<br>
balancing options ran out. Uncomment only one option!<br>
IMPORTANT! Change low pass filter setting changes PID behaviour, so retune your PID's after changing LPF.*/<br>
//#define MPU6050_LPF_256HZ // This is the default setting, no need to uncomment, just for reference<br>
//#define MPU6050_LPF_188HZ<br>
//#define MPU6050_LPF_98HZ<br>
//#define MPU6050_LPF_42HZ<br>
//#define MPU6050_LPF_20HZ<br>
//#define MPU6050_LPF_10HZ<br>
//#define MPU6050_LPF_5HZ // Use this only in extreme cases, rather change motors and/or props<br>
<br>
/****** Gyro smoothing **********************************/<br>
/* GYRO_SMOOTHING. In case you cannot reduce vibrations _and_ _after_ you have tried the low pass filter options, you<br>
may try this gyro smoothing via averaging. Not suitable for multicopters!<br>
Good results for helicopter, airplanes and flying wings (foamies) with lots of vibrations.*/<br>
//#define GYRO_SMOOTHING {20, 20, 3} // separate averaging ranges for roll, pitch, yaw<br>
<br>
/************************ Moving Average Gyros **********************************/<br>
//#define MMGYRO // Active Moving Average Function for Gyros<br>
//#define MMGYROVECTORLENGHT 10 // Lenght of Moving Average Vector<br>
/* Moving Average ServoGimbal Signal Output */<br>
//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal<br>
//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 6 - OPTIONAL FEATURES *******/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/* Pseudo-derivative conrtroller for level mode (experimental)<br>
Additional information: <a href="http://www.multiwii.com/forum/viewtopic.php?f=8&t=503" target="_blank" rel="nofollow">http://www.multiwii.com/forum/viewtopic.php?f=8&t=503</a> */<br>
//#define LEVEL_PDF<br>
<br>
<br>
/******** Failsave settings ********************/<br>
/* Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.<br>
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered<br>
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.<br>
This value is depended from your configuration, AUW and some other params.<br>
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.<br>
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.<br>
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/<br>
//#define FAILSAFE // uncomment to activate the failsafe function<br>
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example<br>
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example<br>
#define FAILSAVE_THROTTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case<br>
<br>
<br>
/***************** DFRobot LED RING *********************************/<br>
/* I2C DFRobot LED RING communication */<br>
//#define LED_RING<br>
<br>
/******************************** LED FLASHER ***********************************/<br>
//#define LED_FLASHER<br>
//#define LED_FLASHER_DDR DDRB<br>
//#define LED_FLASHER_PORT PORTB<br>
//#define LED_FLASHER_BIT PORTB4<br>
//#define LED_FLASHER_SEQUENCE ( (uint8_t) 0 )<br>
// create double flashes<br>
//#define LED_FLASHER_SEQUENCE_ARMED ( (uint8_t) (1<<0 | 1<<2) )<br>
// full illumination<br>
//#define LED_FLASHER_SEQUENCE_MAX 0xFF<br>
<br>
<br>
/******************************* Landing lights *********************************/<br>
/* Landing lights<br>
Use an output pin to control landing lights.<br>
They can be switched automatically when used in conjunction<br>
with altitude data from a sonar unit. */<br>
//#define LANDING_LIGHTS_DDR DDRC<br>
//#define LANDING_LIGHTS_PORT PORTC<br>
//#define LANDING_LIGHTS_BIT PORTC0<br>
<br>
/* altitude above ground (in cm) as reported by sonar */<br>
//#define LANDING_LIGHTS_AUTO_ALTITUDE 50<br>
<br>
<br>
/************************* INFLIGHT ACC Calibration *****************************/<br>
/* This will activate the ACC-Inflight calibration if unchecked */<br>
//#define INFLIGHT_ACC_CALIBRATION<br>
<br>
/************************** Disable WMP power pin *******************************/<br>
/* disable use of the POWER PIN<br>
(allready done if the option RCAUXPIN12 is selected) */<br>
//#define DISABLE_POWER_PIN<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** TX-related **************************/<br>
/**************************************************************************************/<br>
<br>
/* introduce a deadband around the stick center<br>
Must be greater than zero, comment if you dont want a deadband on roll, pitch and yaw */<br>
//#define DEADBAND 6<br>
<br>
/* defines the neutral zone of throttle stick during altitude hold, default setting is<br>
+/-20 uncommend and change the value below if you want to change it. */<br>
//#define ALT_HOLD_THROTTLE_NEUTRAL_ZONE 20 <br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** GPS **************************/<br>
/**************************************************************************************/<br>
<br>
/* GPS using a SERIAL port<br>
only available on MEGA boards (this might be possible on 328 based boards in the future)<br>
if enabled, define here the Arduino Serial port number and the UART speed<br>
note: only the RX PIN is used, the GPS is not configured by multiwii<br>
the GPS must be configured to output GGA and RMC NMEA sentences (which is generally the default conf for most GPS devices)<br>
at least 5Hz update rate. uncomment the first line to select the GPS serial port of the arduino */<br>
//#define GPS_SERIAL 2 // should be 2 for flyduino v2. It's the serial port number on arduino MEGA<br>
#define GPS_BAUD 115200<br>
<br>
//#define GPS_PROMINI_SERIAL 57600 // Will Autosense if GPS is connected when ardu boots<br>
<br>
/* I2C GPS device made with an independant arduino + GPS device<br>
including some navigation functions<br>
contribution from EOSBandi <a href="http://code.google.com/p/i2c-gps-nav/" target="_blank" rel="nofollow">http://code.google.com/p/i2c-gps-nav/</a> <br>
You have to use at least I2CGpsNav code r33 */<br>
#define I2C_GPS<br>
<br>
/* I2C GPS device made with an indeedent ATTiny[24]313 + GPS device and<br>
optional sonar device. <a href="https://github.com/wertarbyte/tiny-gps/" target="_blank" rel="nofollow">https://github.com/wertarbyte/tiny-gps/</a> */<br>
/* get GPS data from Tiny-GPS */<br>
//#define TINY_GPS<br>
/* get sonar data from Tiny-GPS */<br>
//#define TINY_GPS_SONAR<br>
<br>
/* indicate a valid GPS fix with at least 5 satellites by flashing the LED? */<br>
#define GPS_LED_INDICATOR<br>
<br>
/* GPS data readed from OSD -- still need some more code to work */<br>
//#define GPS_FROM_OSD<br>
<br>
//#define USE_MSP_WP //Enables the MSP_WP command, which is used by WinGUI to display and log Home and Poshold positions<br>
//Uncomment it if you are planning to use WinGUI - Will cost +208 bytes of Flash<br>
<br>
//#define DONT_RESET_HOME_AT_ARM // HOME position is reset at every arm, uncomment it to prohibit it (you can set home position with GyroCalibration)<br>
<br>
/* GPS navigation can control the heading */<br>
<br>
#define NAV_CONTROLS_HEADING true // copter faces toward the navigation point, maghold must be enabled for it<br>
#define NAV_TAIL_FIRST false // true - copter comes in with tail first <br>
#define NAV_SET_TAKEOFF_HEADING true // true - when copter arrives to home position it rotates it's head to takeoff direction<br>
<br>
<br>
/* Get your magnetic decliniation from here : <a href="http://magnetic-declination.com/" target="_blank" rel="nofollow">http://magnetic-declination.com/</a><br>
Convert the degree+minutes into decimal degree by ==> degree+minutes*(1/60)<br>
Note the sign on declination it could be negative or positive (WEST or EAST) */<br>
//#define MAG_DECLINIATION 3.96f //For Budapest Hungary.<br>
#define MAG_DECLINIATION 0.0f<br>
<br>
#define GPS_FILTERING // add a 5 element moving average filter to GPS coordinates, helps eliminate gps noise but adds latency comment out to disable<br>
#define GPS_LOW_SPEED_D_FILTER // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise commnent out to disable<br>
#define GPS_WP_RADIUS 200 // if we are within this distance to a waypoint then we consider it reached (distance is in cm)<br>
#define NAV_SLEW_RATE 30 // Adds a rate control to nav output, will smoothen out nav angle spikes<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** LCD/OLED - display settings *********************/<br>
/**************************************************************************************/<br>
<br>
/* uncomment this line if you plan to use a LCD or OLED */<br>
//#define LCD_CONF<br>
<br>
/* to include setting the aux switches for AUX1 -> AUX4 via LCD */ //to review (activate[] is now 16 bit long)<br>
//#define LCD_CONF_AUX<br>
<br>
/* if program gets too large (>32k), need to exclude some functionality */<br>
/* uncomment to suppress some unwanted aux3 aux4 items in config menu (only useful if LCD_CONF_AUX is enabled) */<br>
//#define SUPPRESS_LCD_CONF_AUX34<br>
<br>
/***************************** The type of LCD **********************************/<br>
/* choice of LCD attached for configuration and telemetry, see notes below */<br>
//#define LCD_SERIAL3W // Alex' initial variant with 3 wires, using rx-pin for transmission @9600 baud fixed<br>
//#define LCD_TEXTSTAR // SERIAL LCD: Cat's Whisker LCD_TEXTSTAR Module CW-LCD-02 (Which has 4 input keys for selecting menus)<br>
//#define LCD_VT100 // SERIAL LCD: vt100 compatible terminal emulation (blueterm, putty, etc.)<br>
//#define LCD_ETPP // I2C LCD: Eagle Tree Power Panel LCD, which is i2c (not serial)<br>
//#define LCD_LCD03 // I2C LCD: LCD03, which is i2c<br>
//#define OLED_I2C_128x64 // I2C LCD: OLED <a href="http://www.multiwii.com/forum/viewtopic.php?f=7&t=1350" target="_blank" rel="nofollow">http://www.multiwii.com/forum/viewtopic.php?f=7&t=1350</a><br>
<br>
/****************************** Logo settings ***********************************/<br>
//#define SUPPRESS_OLED_I2C_128x64LOGO // suppress display of OLED logo to save memory<br>
<br>
/* style of display - AUTODETECTED via LCD_ setting - only activate to overwrite defaults */<br>
//#define DISPLAY_2LINES<br>
//#define DISPLAY_MULTILINE<br>
//#define MULTILINE_PRE 2 // multiline configMenu # pref lines<br>
//#define MULTILINE_POST 6 // multiline configMenu # post lines<br>
/******************************** Navigation ***********************************/<br>
/* keys to navigate the LCD menu (preset to LCD_TEXTSTAR key-depress codes)*/<br>
#define LCD_MENU_PREV 'a'<br>
#define LCD_MENU_NEXT 'c'<br>
#define LCD_VALUE_UP 'd'<br>
#define LCD_VALUE_DOWN 'b'<br>
<br>
#define LCD_MENU_SAVE_EXIT 's'<br>
#define LCD_MENU_ABORT 'x'<br>
<br>
/* To use an LCD03 for configuration:<br>
<a href="http://www.robot-electronics.co.uk/htm/Lcd03tech.htm" target="_blank" rel="nofollow">http://www.robot-electronics.co.uk/htm/Lcd03tech.htm</a><br>
Remove the jumper on its back to set i2c control.<br>
VCC to +5V VCC (pin1 from top)<br>
SDA - Pin A4 Mini Pro - Pin 20 Mega (pin2 from top)<br>
SCL - Pin A5 Mini Pro - Pin 21 Mega (pin3 from top)<br>
GND to Ground (pin4 from top)*/<br>
<br>
/* To use an Eagle Tree Power Panel LCD for configuration:<br>
White wire to Ground<br>
Red wire to +5V VCC (or to the WMP power pin, if you prefer to reset everything on the bus when WMP resets)<br>
Yellow wire to SDA - Pin A4 Mini Pro - Pin 20 Mega<br>
Brown wire to SCL - Pin A5 Mini Pro - Pin 21 Mega */<br>
<br>
/* Cat's whisker LCD_TEXTSTAR LCD<br>
Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD )<br>
Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode<br>
NO rx / tx line reconfiguration, use natural pins.<br>
The four buttons sending 'A', 'B', 'C', 'D' are supported for configuration navigation and request of telemetry pages 1-4 */<br>
<br>
<br>
/**************************************************************************************/<br>
/*********************** telemetry **************************/<br>
/**************************************************************************************/<br>
<br>
/* to monitor system values (battery level, loop time etc. with LCD enable this<br>
note: for now you must send single characters to request different pages<br>
Buttons toggle request for page on/off<br>
The active page on the LCD does get updated automatically<br>
Easy to use with Terminal application or display like LCD - if available uses the 4 preconfigured buttons to send 'A', 'B', 'C', 'D' */<br>
/******************************** Activation ***********************************/<br>
//#define LCD_TELEMETRY<br>
<br>
/* to enable automatic hopping between a choice of telemetry pages uncomment this.<br>
This may be useful if your LCD has no buttons or the sending is broken<br>
hopping is activated and deactivated in unarmed mode with throttle=low & roll=left & pitch=forward<br>
set it to the sequence of telemetry pages you want to see<br>
2 line displays support pages 1-9<br>
multiline displays support pages 1-5 */<br>
//#define LCD_TELEMETRY_AUTO "123452679" // pages 1 to 7 in ascending order<br>
//#define LCD_TELEMETRY_AUTO "212232425262729" // strong emphasis on page 2<br>
<br>
/* same as above, but manual stepping sequence; requires <br>
stick input (throttle=low & roll=right & pitch=forward) to <br>
step through each defined telemetry page */<br>
//#define LCD_TELEMETRY_STEP "0123456789" // must begin with 0<br>
<br>
<br>
/* on telemetry page B (2) it gives a bar graph which shows how much voltage battery has left. Range from 0 to 12 Volt is not very informative<br>
so we try do define a meaningful range. For a 3S battery we define full=12,6V and calculate how much it is above first warning level<br>
Example: 12.6V - VBATLEVEL1_3S (for me = 126 - 102 = 24) */<br>
#define VBATREF 24<br>
<br>
/* if program gets too large (>32k), need to exclude some functionality<br>
uncomment to suppress some unwanted telemetry pages (only useful if telemetry is enabled) */<br>
//#define SUPPRESS_TELEMETRY_PAGE_1<br>
//#define SUPPRESS_TELEMETRY_PAGE_2<br>
//#define SUPPRESS_TELEMETRY_PAGE_3<br>
//#define SUPPRESS_TELEMETRY_PAGE_4<br>
//#define SUPPRESS_TELEMETRY_PAGE_5<br>
//#define SUPPRESS_TELEMETRY_PAGE_6<br>
//#define SUPPRESS_TELEMETRY_PAGE_7<br>
//#define SUPPRESS_TELEMETRY_PAGE_8<br>
//#define SUPPRESS_TELEMETRY_PAGE_9<br>
<br>
/********************************************************************/<br>
/**** Buzzer ****/<br>
/********************************************************************/<br>
//#define BUZZER<br>
//#define RCOPTIONSBEEP //uncomment this if you want the buzzer to beep at any rcOptions change on channel Aux1 to Aux4<br>
//#define ARMEDTIMEWARNING 330 // Trigger an alarm after a certain time of being armed [s] to save you lipo (if your TX does not have a countdown)<br>
<br>
/********************************************************************/<br>
/**** battery voltage monitoring ****/<br>
/********************************************************************/<br>
/* for V BAT monitoring<br>
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN<br>
with R1=33k and R2=51k<br>
vbat = [0;1023]*16/VBATSCALE<br>
must be associated with #define BUZZER ! */<br>
//#define VBAT // uncomment this line to activate the vbat code<br>
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage<br>
#define VBATLEVEL1_3S 107 // 10,7V<br>
#define VBATLEVEL2_3S 103 // 10,3V<br>
#define VBATLEVEL3_3S 99 // 9.9V<br>
#define NO_VBAT 16 // Avoid beeping without any battery<br>
<br>
<br>
/********************************************************************/<br>
/**** powermeter (battery capacity monitoring) ****/<br>
/********************************************************************/<br>
<br>
/* enable monitoring of the power consumption from battery (think of mAh)<br>
allows to set alarm value in GUI or via LCD<br>
Two options:<br>
1 - soft: - (good results +-5% for plush and mystery ESCs @ 2S and 3S, not good with SuperSimple ESC<br>
00. relies on your combo of battery type (Voltage, cpacity), ESC, ESC settings, motors, props and multiwii cycle time<br>
01. set POWERMETER soft. Uses PLEVELSCALE = 50, PLEVELDIV = PLEVELDIVSOFT = 5000<br>
0. output is a value that linearily scales to power (mAh)<br>
1. get voltage reading right first<br>
2. start with freshly charged battery<br>
3. go fly your typical flight (routine and duration)<br>
4. at end connect to GUI or LCD and read the power value; write it down (example 4711)<br>
5. charge battery, write down amount of energy needed (example 722 mAh)<br>
6. compute alarm value for desired power threshold (example 750 mAh : alarm = 4711 / 722 * 750)<br>
7. set alarm value in GUI or LCD<br>
8. enjoy your new battery alarm - possibly repeat steps 2 .. 7<br>
9. if you want the numbers to represent your mAh value, you must change PLEVELDIV<br>
2 - hard: - (uses hardware sensor, after configuration gives reasonable results<br>
00. uses analog pin 2 to read voltage output from sensor.<br>
01. set POWERMETER hard. Uses PLEVELSCALE = 50<br>
02. install low path filter for 25 Hz to sensor input<br>
03. check your average cycle time. If not close to 3ms, then you must change PLEVELDIV accordingly<br>
1. compute PLEVELDIV for your sensor (see below for insturctions)<br>
2. set PLEVELDIVSOFT to 5000 ( to use LOG_VALUES for individual motor comparison)<br>
3. attach, set PSENSORNULL and PINT2mA<br>
4. configure, compile, upload, set alarm value in GUI or LCD<br>
3. enjoy true readings of mAh consumed<br>
set POWERMETER to "soft" (1) or "hard" (2) depending on sensor you want to utilize */<br>
//#define POWERMETER_SOFT<br>
//#define POWERMETER_HARD<br>
/* the sum of all powermeters ranges from [0:60000 e4] theoretically.<br>
the alarm level from eeprom is out of [0:255], so we multipy alarm level with PLEVELSCALE and with 1e4 before comparing<br>
PLEVELSCALE is the step size you can use to set alarm */<br>
#define PLEVELSCALE 50 // if you change this value for other granularity, you must search for comments in code to change accordingly<br>
/* larger PLEVELDIV will get you smaller value for power (mAh equivalent) */<br>
#define PLEVELDIV 5000 // default for soft - if you lower PLEVELDIV, beware of overrun in uint32 pMeter<br>
#define PLEVELDIVSOFT PLEVELDIV // for soft always equal to PLEVELDIV; for hard set to 5000<br>
//#define PLEVELDIV 1361L // to convert the sum into mAh divide by this value<br>
/* amploc 25A sensor has 37mV/A<br>
arduino analog resolution is 4.9mV per unit; units from [0..1023]<br>
sampling rate 20ms, approx 19977 micro seconds<br>
PLEVELDIV = 37 / 4.9 * 10e6 / 18000 * 3600 / 1000 = 1361L<br>
set to analogRead() value for zero current */<br>
#define PSENSORNULL 510 // for I=0A my sensor gives 1/2 Vss; that is approx 2.49Volt<br>
#define PINT2mA 13 // for telemtry display: one integer step on arduino analog translates to mA (example 4.9 / 37 * 100<br>
<br>
<br>
<br>
<br>
/*************************************************************************************************/<br>
/***************** ***************/<br>
/**************** SECTION 7 - TUNING & DEVELOPER **************/<br>
/***************** ***************/<br>
/*************************************************************************************************/<br>
<br>
/**************************************************************************************/<br>
/******** special ESC with extended range [0-2000] microseconds ********************/<br>
/**************************************************************************************/<br>
//#define EXT_MOTOR_RANGE<br>
<br>
/**************************************************************************************/<br>
/*********************** motor, servo and other presets ***********************/<br>
/**************************************************************************************/<br>
/* motors will not spin when the throttle command is in low position<br>
this is an alternative method to stop immediately the motors */<br>
//#define MOTOR_STOP<br>
<br>
/* some radios have not a neutral point centered on 1500. can be changed here */<br>
#define MIDRC 1500<br>
<br>
/*********************** Servo Refreshrates ***********************/<br>
/* Default 50Hz Servo refresh rate*/<br>
#define SERVO_RFR_50HZ<br>
<br>
/* up to 160Hz servo refreshrate .. works with the most analog servos*/<br>
//#define SERVO_RFR_160HZ<br>
<br>
/* up to 300Hz refreshrate it is as fast as possible (100-300Hz depending on the cound of used servos and the servos state).<br>
for use with digital servos<br>
dont use it with analog servos! thay may get damage. (some will work but be careful) */<br>
//#define SERVO_RFR_300HZ<br>
<br>
/*********************** HW PWM Servos ***********************/ <br>
/* HW PWM Gimbal for Arduino Mega.. moves:<br>
Pitch = pin 44<br>
Roll = pin 45<br>
this reduces the PWM resolution for all other servos to 8 bit */<br>
//#define MEGA_HW_GIMBAL<br>
<br>
/********************************************************************/<br>
/**** IMU complimentary filter tuning ****/<br>
/********************************************************************/<br>
<br>
/* Set the Low Pass Filter factor for ACC<br>
Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define ACC_LPF_FACTOR 100<br>
<br>
/* Set the Low Pass Filter factor for Magnetometer<br>
Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define MG_LPF_FACTOR 4<br>
<br>
/* Set the Gyro Weight for Gyro/Acc complementary filter<br>
Increasing this value would reduce and delay Acc influence on the output of the filter<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define GYR_CMPF_FACTOR 400.0f<br>
<br>
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter<br>
Increasing this value would reduce and delay Magnetometer influence on the output of the filter<br>
Comment this out if you want to set a specific coeff (non default)*/<br>
//#define GYR_CMPFM_FACTOR 200.0f<br>
<br>
<br>
/********************************************************************/<br>
/**** diagnostics ****/<br>
/********************************************************************/<br>
<br>
/* to log values like max loop time and others to come<br>
logging values are visible via LCD config<br>
set to 2, if you want powerconsumption on a per motor basis (this uses the big array and is a memory hog, if POWERMETER <> PM_SOFT) */<br>
//#define LOG_VALUES 1<br>
<br>
/* to add debugging code<br>
not needed and not recommended for normal operation<br>
will add extra code that may slow down the main loop or make copter non-flyable */<br>
//#define DEBUG<br>
<br>
/* Use this to trigger LCD configuration without a TX - only for debugging - do NOT fly with this activated */<br>
//#define LCD_CONF_DEBUG<br>
<br>
/* Use this to trigger telemetry without a TX - only for debugging - do NOT fly with this activated */<br>
//#define LCD_TELEMETRY_DEBUG //This form rolls between all screens, LCD_TELEMETRY_AUTO must also be defined.<br>
//#define LCD_TELEMETRY_DEBUG 6 //This form stays on the screen specified.<br>
<br>
<br>
/********************************************************************/<br>
/**** ESCs calibration ****/<br>
/********************************************************************/<br>
<br>
/* to calibrate all ESCs connected to MWii at the same time (useful to avoid unplugging/re-plugging each ESC)<br>
Warning: this creates a special version of MultiWii Code<br>
You cannot fly with this special version. It is only to be used for calibrating ESCs<br>
Read How To at <a href="http://code.google.com/p/multiwii/wiki/ESCsCalibration" target="_blank" rel="nofollow">http://code.google.com/p/multiwii/wiki/ESCsCalibration</a> */<br>
#define ESC_CALIB_LOW MINCOMMAND<br>
#define ESC_CALIB_HIGH 2000<br>
//#define ESC_CALIB_CANNOT_FLY // uncomment to activate<br>
<br>
/**** internal frequencies ****/<br>
/* frequenies for rare cyclic actions in the main loop, depend on cycle time<br>
time base is main loop cycle time - a value of 6 means to trigger the action every 6th run through the main loop<br>
example: with cycle time of approx 3ms, do action every 6*3ms=18ms<br>
value must be [1; 65535] */<br>
#define LCD_TELEMETRY_FREQ 23 // to send telemetry data over serial 23 <=> 60ms <=> 16Hz (only sending interlaced, so 8Hz update rate)<br>
#define LCD_TELEMETRY_AUTO_FREQ 667 // to step to next telemetry page 667 <=> 2s<br>
#define PSENSORFREQ 6 // to read hardware powermeter sensor 6 <=> 18ms<br>
#define VBATFREQ PSENSORFREQ // to read battery voltage - keep equal to PSENSORFREQ unless you know what you are doing<br>
<br>
/********************************************************************/<br>
/**** Regression testing ****/<br>
/********************************************************************/<br>
<br>
/* for development only:<br>
to allow for easier and reproducable config sets for test compiling, different sets of config parameters are kept<br>
together. This is meant to help detecting compile time errors for various features in a coordinated way.<br>
It is not meant to produce your flying firmware<br>
To use:<br>
- do not set any options in config.h,<br>
- enable with #define COPTERTEST 1, then compile<br>
- if possible, check for the size<br>
- repeat with other values of 2, 3, 4 etc.<br>
*/<br>
//#define COPTERTEST 1<br>
<br>
/*************************************************************************************************/<br>
/**** END OF CONFIGURABLE PARAMETERS ****/<br>
/*************************************************************************************************/<br>
<br>
<font color="#FF0000">***************************************************</font><br>
<br><br>
GPS wird in der GUI grün angezeigt. Ich habe I2C Errors und finde keine Sateliten.<br>
Das GPS Modul leuchted dauergrün, das NavBoard blinkt smoth im Sekundenanstand.<br>
Muss der GPS Empfänger selber auch geflashed werden ? Ich habe noch nirgendswo gefunden, wo ich im Arduino beim NavBoard zwischen der Hz Zahlen wählen kann (5 oder 10Hz)<br>
<br>
Wer weitere Infos braucht, einfach melden, gerne können wir auch telefonieren, wenn du in der Umgebung von Gütersloh/ Bielefeld wohnst komme ich auch gerne mit dem ganzen Krempel vorbei.<br>
Gruß<br>
Mirco
Zuletzt bearbeitet von einem Moderator:
Das möchte ich natürlich nicht !!
