Arduino Quadcopter

Código

• MultiWii.ino

MultiWii.ino C / C ++

 #include "Arduino.h" #include "config.h" #include "def.h" #include "types.h" #include "GPS.h" #include "Serial.h" #include "Sensores. h "#include" MultiWii.h "#include" EEPROM.h "#include  #if GPS // Prototipos de función para otras funciones de GPS // Estos quizás podrían ir al archivo gps.h, sin embargo estos son local al gps.cpp static void GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * Bearing); static void GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2 , uint32_t * dist); vacío estático GPS_calc_velocity (void); vacío estático GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); vacío estático GPS_calc_intposhold_lng (uidol_desde_sp_sp_gps); ); static void GPS_calc_nav_rate (uint16_t max_speed); int32_t wrap_18000 (int32_t ang); static void check_missed_wp (void); void GPS_calc_longitude_scaling (int32_t lat); static void GPS_update_crosstrack (int32_wp); p_36000 (int32_t ang); // Filtro Leadig - TODO:reescribir a C normal en lugar de C ++ // Configurar gps lag # si está definido (UBLOX) || definido (MTK_BINARY19) #define GPS_LAG 0.5f // UBLOX GPS tiene un retraso menor que MTK y otro # else # define GPS_LAG 1.0f // Suponemos que MTK GPS tiene un retraso de 1 segundo # endif static int32_t GPS_coord_lead [2]; // Clase de coordenadas gps filtrada de plomo LeadFilter {public:LeadFilter ():_last_velocity (0) {} // configura los valores de radio mínimo y máximo en CLI int32_t get_position (int32_t pos, int16_t vel, float lag_in_seconds =1.0); void clear () {_last_velocity =0; } privado:int16_t _last_velocity;}; int32_t LeadFilter ::get_position (int32_t pos, int16_t vel, float lag_in_seconds) {int16_t accel_contribution =(vel - _last_velocity) * lag_in_seconds * lag_in_seconds; int16_t vel_contribution =vel * lag_in_seconds; // almacena la velocidad para la siguiente iteración _last_velocity =vel; return pos + vel_contribution + accel_contribution;} LeadFilter xLeadFilter; // Filtro de retardo GPS largo LeadFilter yLeadFilter; // Lat GPS lag filter typedef struct PID_PARAM_ {float kP; flotar kI; float kD; flotar Imax; } PID_PARAM; PID_PARAM posholdPID_PARAM; PID_PARAM poshold_ratePID_PARAM; PID_PARAM navPID_PARAM; typedef struct PID_ {integrador flotante; // valor del integrador int32_t last_input; // última entrada para la derivada flotante lastderivative; // última derivada para la salida flotante del filtro de paso bajo; derivada flotante;} PID; PID posholdPID [2]; PID poshold_ratePID [2]; PID navPID [2]; int32_t get_P (int32_t error, struct PID_PARAM_ * pid) {return (float) error * pid-> kP;} int32_t get_I (error int32_t, float * dt, struct PID_ * pid, struct PID_PARAM_ * pid_param) {pid-> integrator + =((float) error * pid_param-> kI) * * dt; pid-> integrador =restringir (pid-> integrador, -pid_param-> Imax, pid_param-> Imax); return pid-> integrator;} int32_t get_D (int32_t input, float * dt, struct PID_ * pid, struct PID_PARAM_ * pid_param) {// dt en milisegundos pid-> derivative =(input - pid-> last_input) / * dt; /// Frecuencia de corte del filtro de paso bajo para el cálculo de la derivada. filtro flotante =7.9577e-3; // Establecer en "1 / (2 * PI * f_cut)"; // Ejemplos de _filter:// f_cut =10 Hz -> _filter =15.9155e-3 // f_cut =15 Hz -> _filter =10.6103e-3 // f_cut =20 Hz -> _filter =7.9577e-3 // f_cut =25 Hz -> _filter =6.3662e-3 // f_cut =30 Hz -> _filter =5.3052e-3 // filtro de paso bajo discreto, corta el // ruido de alta frecuencia que puede volver loco al controlador pid-> derivative =pid-> lastderivative + (* dt / (filter + * dt)) * (pid-> derivative - pid-> lastderivative); // actualiza el estado pid-> last_input =input; pid-> lastderivative =pid-> derivative; // añadir el componente derivado return pid_param-> kD * pid-> derivative;} void reset_PID (struct PID_ * pid) {pid-> integrator =0; pid-> last_input =0; pid-> lastderivative =0;} # define _X 1 # define _Y 0 # define RADX100 0.000174532925 uint8_t land_detect; // Detecta tierra (externa) estática uint32_t land_settle_timer; uint8_t GPS_Frame; // se detectó un GPS_Frame válido y los datos están listos para el cálculo de navegación estático float dTnav; // Delta Time en milisegundos para los cálculos de navegación, actualizado con cada buena lectura de GPS estática int16_t actual_speed [2] ={0,0}; static float GPS_scaleLonDown; // esto se usa para compensar la reducción de la longitud a medida que avanzamos hacia los polos // La diferencia entre la tasa de viaje deseada y la tasa de viaje real // actualizado después de la lectura del GPS - 5-10 hzstatic int16_t rate_error [2]; static int32_t error [2]; static int32_t GPS_WP [2]; // Usado actualmente WPstatic int32_t GPS_FROM [2]; // el punto de ruta anterior para un seguimiento preciso siguienteint32_t target_bearing; // Este es el ángulo desde el helicóptero hasta la ubicación "next_WP" en grados * 100static int32_t original_target_bearing; // deg * 100, El ángulo original al next_WP cuando se configuró el next_WP, También se usa para verificar cuando pasamos un WPstatic int16_t crosstrack_error; // La cantidad de corrección de ángulo aplicada a target_bearing para que el helicóptero vuelva a su pathuint32_t óptimo wp_distance; // distancia entre el plano y next_WP en cmstatic uint16_t waypoint_speed_gov; // se utiliza para la velocidad lenta cuando se inicia la navegación; //////////////////////////////////////// //////////////////////////////////////////////// media móvil variables de filtro // # define GPS_FILTER_VECTOR_LENGTH 5static uint8_t GPS_filter_index =0; static int32_t GPS_filter [2] [GPS_FILTER_VECTOR_LENGTH]; static int32_t GPS_filter_sum [2]; static int32_t GPS_read [2]; static int32_t GPS_read [2]; static int32_t GPS_read [2];; // el grado de latitud sin decimales (lat / 10000000) static uint16_t fracción3 [2]; static int16_t nav_takeoff_bearing; // guarda el rumbo en el despegue (1deg =1) usado para rotar a la dirección de despegue cuando llega a casa // Procesador de navegación principal y motor de estado // TODO:agregar estados en proceso para facilitar la carga de procesamiento uint8_t GPS_Compute (void) {eje de caracteres sin firmar; uint32_t dist; // variable temporal para almacenar dist to copter int32_t dir; // variable temporal para almacenar dir to copter static uint32_t nav_loopTimer; // verificamos que tenemos un marco válido, si no es así, regresa inmediatamente if (GPS_Frame ==0) return 0; else GPS_Frame =0; // verifica la posición de inicio y configúrala si no se estableció if (f.GPS_FIX &&GPS_numSat> =5) {#if! defined (DONT_RESET_HOME_AT_ARM) if (! f.ARMED) {f.GPS_FIX_HOME =0;} #endif if (! f.GPS_FIX_HOME &&f.ARMED) {GPS_reset_home_position (); } // Aplicar el filtro de media móvil a los datos de GPS si (GPS_conf.filtering) {GPS_filter_index =(GPS_filter_index + 1)% GPS_FILTER_VECTOR_LENGTH; para (eje =0; eje <2; eje ++) {GPS_read [eje] =GPS_coord [eje]; // Los últimos datos sin filtrar están en GPS_latitude y GPS_longitude GPS_degree [axis] =GPS_read [axis] / 10000000; // obtener el grado para asegurar que la suma se ajuste a int32_t // ¿Qué tan cerca estamos de una línea de grados? son los primeros tres dígitos de las fracciones de grado // luego lo usamos para verificar si estamos cerca de una línea de grado, si es así, deshabilitar el promedio, fracción3 [eje] =(lectura_GPS [eje] - grado_GPS [eje] * 10000000 ) / 10000; GPS_filter_sum [eje] - =GPS_filter [eje] [GPS_filter_index]; GPS_filter [eje] [GPS_filter_index] =GPS_read [eje] - (GPS_degree [eje] * 10000000); GPS_filter_sum [eje] + =GPS_filter [eje] [GPS_filter_index]; GPS_filtered [eje] =GPS_filter_sum [eje] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree [eje] * 10000000); if (NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) {// usamos el promedio de GPS solo en modo poshold ... if (fracción3 [eje]> 1 &&fracción3 [eje] <999) GPS_coord [eje] =GPS_filtrado [ eje]; }}} // Cálculo de dTnav // Tiempo para calcular la velocidad x, y y los pids de navegación dTnav =(float) (millis () - nav_loopTimer) / 1000.0; nav_loopTimer =millis (); // evitar la ejecución de un GPS defectuoso dTnav =min (dTnav, 1.0); // calcula la distancia y los rumbos para la interfaz gráfica de usuario y otras cosas continuamente - Desde casa hasta el helicóptero GPS_bearing (&GPS_coord [LAT], &GPS_coord [LON], &GPS_home [LAT], &GPS_home [LON], &dir); GPS_distance_cm (&GPS_coord [LAT], &GPS_coord [LON], &GPS_home [LAT], &GPS_home [LON], &dist); GPS_distanceToHome =dist / 100; GPS_directionToHome =dir / 100; if (! f.GPS_FIX_HOME) {// Si no tenemos configurado el inicio, no mostramos nada GPS_distanceToHome =0; GPS_directionToHome =0; } // Verifique la configuración de la cerca y ejecute RTH si es necesario // TODO:autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; descanso; caso NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold (); // Aterrizar en posición mantenga f.THROTTLE_IGNORED =1; // Ignorar la entrada de la palanca del acelerador f.GPS_BARO_MODE =1; // Toma el control del modo BARO land_detect =0; // Restablecer detector de tierra f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Marcar el proceso de tierra NAV_state =NAV_STATE_LAND_IN_PROGRESS; descanso; caso NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold (); check_land (); // Llamar al detector de tierra si (f.LAND_COMPLETED) {nav_timer_stop =millis () + 5000; NAV_state =NAV_STATE_LANDED; } descanso; case NAV_STATE_LANDED:// Desarmar si la palanca del ACELERADOR está al mínimo o 5 segundos después de la tierra detectada si (rcData [ACELERADOR]  0) {// si no se llega a cero, haz un salto next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } descanso; case NAV_STATE_PROCESS_NEXT:// Procesando el siguiente paso de la misión NAV_error =NAV_ERROR_NONE; if (! recallWP (next_step)) {abort_mission (NAV_ERROR_WP_CRC); } else {switch (mission_step.action) {// Los comandos Waypoiny y hold comienzan con un estado en ruta también incluye el comando LAND case MISSION_WAYPOINT:case MISSION_HOLD_TIME:case MISSION_HOLD_UNLIM:case MISSION_LAND:set_new_altitude (mission_step.altitude); GPS_set_next_wp (&mission_step.pos [LAT], &mission_step.pos [LON], &GPS_prev [LAT], &GPS_prev [LON]); if ((wp_distance / 100)> =GPS_conf.safe_wp_distance) abort_mission (NAV_ERROR_TOOFAR); más NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev [LAT] =mission_step.pos [LAT]; // Guarde las coordenadas wp para una ruta precisa calc GPS_prev [LON] =mission_step.pos [LON]; descanso; case MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) // si config y mission_step alt es cero set_new_altitude (alt.EstAlt); // RTH regresa a la altitud real else {uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; // la altitud en el paso de la misión tiene prioridad else rth_alt =mission_step.altitude; if (alt.EstAlt  0 &&mission_step.parameter1  GPS_conf.nav_max_altitude * 100) _new_alt =GPS_conf.nav_max_altitude * 100; if (_new_alt ==alt.EstAlt) {force_new_altitude (_new_alt); regreso; } // Comenzamos en la altitud de ubicación actual y cambiamos gradualmente alt alt_to_hold =alt.EstAlt; // para calcular el tiempo delta alt_change_timer =millis (); // guarda la altitud objetivo target_altitude =_new_alt; // reiniciamos nuestro integrador de altitud alt_change =0; // guarda la altitud original original_altitude =alt.EstAlt; // para decidir si hemos alcanzado la altitud objetivo if (target_altitude> original_altitude) {// estamos abajo, subiendo alt_change_flag =ASCENDING; } else if (target_altitude  =target_altitude) alt_change_flag =REACHED_ALT; // no deberíamos mandar más allá de nuestro objetivo if (alt_to_hold> =target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) {// estamos arriba, bajando if (alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // No deberíamos mandar más allá de nuestro objetivo if (alt_to_hold <=target_altitude) return target_altitude; } // si hemos alcanzado nuestra altitud objetivo, devolvemos el objetivo alt if (alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs (alt_to_hold - target_altitude); // escala es cómo generamos una tasa deseada a partir del tiempo transcurrido // una escala más pequeña significa tasas más rápidas int8_t _scale =4; if (alt_to_hold > 4 =64cm / s descenso por defecto int32_t change =(millis () - alt_change_timer)>> _scale; if (alt_change_flag ==ASCENDING) {alt_change + =cambiar; } else {alt_change - =cambio; } // para generar tiempo delta alt_change_timer =millis (); return original_altitude + alt_change;} ////////////////////////////////////////////// ////////////////////////////////////////// Funciones de navegación GPS basadas en PID // Autor :EOSBandi // Basado en código e ideas del equipo de Arducopter:Jason Short, Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen // Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni // restricción original no funciona con variablesint16_t constrain_int16 (int16_t amt, int16_t low, int16_t high) {return ((amt) <(low)? (low):((amt)> (high)? (high) :( amt))); } /////////////////////////////////////////////////// ///////////////////////////////////// esto se utiliza para compensar la reducción de la longitud a medida que avanzamos hacia la polos // Está bien calcular esto una vez por configuración de waypoint, ya que cambia un poco dentro del alcance de un multicóptero // void GPS_calc_longitude_scaling (int32_t lat) {GPS_scaleLonDown =cos (lat * 1.0e-7f * 0.01745329251f);} / //////////////////////////////////////////////////// /////////////////////////// ////////// Establece el waypoint para navegar, restablecer las variables necesarias y calcular los valores iniciales // void GPS_set_next_wp (int32_t * lat_to, int32_t * lon_to, int32_t * lat_from, int32_t * lon_from) {GPS_WP [LAT] =* lat_to; GPS_WP [LON] =* lon_to; GPS_FROM [LAT] =* lat_from; GPS_FROM [LON] =* lon_from; GPS_calc_longitude_scaling (* lat_to); GPS_bearing (&GPS_FROM [LAT], &GPS_FROM [LON], &GPS_WP [LAT], &GPS_WP [LON], &target_bearing); GPS_distance_cm (&GPS_FROM [LAT], &GPS_FROM [LON], &GPS_WP [LAT], &GPS_WP [LON], &wp_distance); GPS_calc_location_error (&GPS_WP [LAT], &GPS_WP [LON], &GPS_FROM [LAT], &GPS_FROM [LON]); waypoint_speed_gov =GPS_conf.nav_speed_min; Origen_objetivo_original =objetivo_bearing;} /////////////////////////////////////////////// ///////////////////////////////////////// Compruebe si nos perdimos el destino de alguna manera // static bool check_missed_wp (void) {int32_t temp; temp =target_bearing - original_target_bearing; temp =wrap_18000 (temp); return (abs (temp)> 10000); // pasamos el waypoint en 100 grados} ////////////////////////////////////////// //////////////////////////////////////////////// Calcule la distancia entre dos puntos en cm // Obtener rumbo de pos1 a pos2, devuelve un 1deg =100 precisionvoid GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * rumbo) {int32_t off_x =* lon2 - * lon1; int32_t off_y =(* lat2 - * lat1) / GPS_scaleLonDown; * rumbo =9000 + atan2 (-off_y, off_x) * 5729.57795f; // Convierta los redianes de salida a 100xdeg si (* rumbo <0) * rumbo + =36000;} void GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist) {float dLat =( flotar) (* lat2 - * lat1); // diferencia de latitud en 1/10 000 000 grados float dLon =(float) (* lon2 - * lon1) * GPS_scaleLonDown; // x * dist =sqrt (sq (dLat) + sq (dLon)) * 1.11318845f;} // ************************* *********************************************** *************************** // calc_velocity_and_filtered_position - velocidad en direcciones lon y lat calculada a partir de la posición GPS // y datos del acelerómetro // lon_speed expresado en cm / s. los números positivos significan moverse hacia el este // lat_speed expresado en cm / s. números positivos al movernos hacia el norte // Nota:usamos ubicaciones de GPS directamente para calcular la velocidad en lugar de pedir velocidad a GPS porque // esto es más preciso por debajo de 1,5 m / s // Nota:aunque las posiciones se proyectan usando un filtro de plomo, las velocidades se calculan // a partir de las ubicaciones gps inalteradas. No queremos que el ruido de nuestro filtro de plomo afecte la velocidad // *********************************** *********************************************** **************** static void GPS_calc_velocity (void) {static int16_t speed_old [2] ={0,0}; static int32_t last [2] ={0,0}; estático uint8_t init =0; if (init) {float tmp =1.0 / dTnav; actual_speed [_X] =(flotante) (GPS_coord [LON] - último [LON]) * GPS_scaleLonDown * tmp; actual_speed [_Y] =(flotante) (GPS_coord [LAT] - último [LAT]) * tmp; // TODO:Verifique los cambios de velocidad poco realistas y la navegación de la señal sobre la posible degradación de la señal del GPS si (! GPS_conf.lead_filter) {actual_speed [_X] =(actual_speed [_X] + speed_old [_X]) / 2; velocidad_actual [_Y] =(velocidad_actual [_Y] + velocidad_antigua [_Y]) / 2; speed_old [_X] =actual_speed [_X]; speed_old [_Y] =actual_speed [_Y]; }} init =1; último [LON] =GPS_coord [LON]; último [LAT] =GPS_coord [LAT]; if (GPS_conf.lead_filter) {GPS_coord_lead [LON] =xLeadFilter.get_position (GPS_coord [LON], actual_speed [_X], GPS_LAG); GPS_coord_lead [LAT] =yLeadFilter.get_position (GPS_coord [LAT], actual_speed [_Y], GPS_LAG); }} ////////////////////////////////////////////////// ////////////////////////////////////// Calcular un error de ubicación entre dos coordenadas gps // Porque nosotros estamos usando lat y lon para hacer nuestros errores de distancia aquí hay un gráfico rápido:// 100 =1m // 1000 =11m =36 pies // 1800 =19.80m =60 pies // 3000 =33m // 10000 =111m // estático void GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng) {error [LON] =(float) (* target_lng - * gps_lng) * GPS_scaleLonDown; // X Error error [LAT] =* target_lat - * gps_lat; // Error Y} /////////////////////////////////////////////// ///////////////////////////////////////// Calcular nav_lat y nav_lon a partir de xey error y la velocidad //// TODO:compruebe que la restricción de velocidad del objetivo poshold puede aumentarse para un poshold más rápido lockstatic void GPS_calc_poshold (void) {int32_t d; int32_t target_speed; eje uint8_t; para (eje =0; eje <2; eje ++) {target_speed =get_P (error [eje], &posholdPID_PARAM); // calcula la velocidad deseada a partir del error lat / lon target_speed =constrain (target_speed, -100,100); // Restringir la velocidad objetivo en modo poshold a 1 m / s ayuda a evitar fugas .. rate_error [axis] =target_speed - actual_speed [axis]; // calcula el error de velocidad nav [axis] =get_P (rate_error [axis], &poshold_ratePID_PARAM) + get_I (rate_error [axis] + error [axis], &dTnav, &poshold_ratePID [axis], &poshold_ratePID_PARAM); d =get_D (error [eje], &dTnav, &poshold_ratePID [eje], &poshold_ratePID_PARAM); d =restricción (d, -2000, 2000); // deshacerse del ruido if (abs (actual_speed [eje]) <50) d =0; nav [eje] + =d; // nav [eje] =restringir (nav [eje], -NAV_BANK_MAX, NAV_BANK_MAX); nav [eje] =constrain_int16 (nav [eje], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID [eje] .integrator =poshold_ratePID [eje] .integrator; }} ////////////////////////////////////////////////// ////////////////////////////////////// Calcule el nav_lat y nav_lon deseados para vuelos de distancia como RTH y WP // vacío estático GPS_calc_nav_rate (uint16_t max_speed) {float trig [2]; int32_t target_speed [2]; int32_t tilt; eje uint8_t; GPS_update_crosstrack (); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); // comprobar ¿está bien? velocidad_cruzada =restricción (velocidad_cruzada, -200,200); cross_speed =-cross_speed; temperatura de flotación =(9000l - target_bearing) * RADX100; trig [_X] =cos (temp); trig [_Y] =sin (temp); velocidad_objetivo [_X] =velocidad_máxima * trig [_X] - velocidad_cruzada * trig [_Y]; velocidad_objetivo [_Y] =velocidad_cruzada * trig [_X] + velocidad_máxima * trig [_Y]; para (eje =0; eje <2; eje ++) {rate_error [eje] =target_speed [eje] - actual_speed [eje]; rate_error [eje] =restringir (rate_error [eje], - 1000,1000); nav [eje] =get_P (rate_error [eje], &navPID_PARAM) + get_I (rate_error [eje], &dTnav, &navPID [eje], &navPID_PARAM) + get_D (rate_error [eje], &dTnav, &navPID [eje], &navPID_PARAM); // nav [eje] =restringir (nav [eje], - NAV_BANK_MAX, NAV_BANK_MAX); nav [eje] =constrain_int16 (nav [eje], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID [eje] .integrator =navPID [eje] .integrator; }} static void GPS_update_crosstrack (void) {// Error de crosstrack // ---------------- // Si estamos demasiado lejos o demasiado cerca, no hacemos seguimiento siguiendo el flotador temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin (temp) * wp_distance; // Metros estamos fuera de la línea de seguimiento} /////////////////////////////////////////// ///////////////////////////////////////////// Determine la velocidad deseada al navegar hacia un waypoint, también implemente un aumento lento // de velocidad al iniciar una navegación //// |  waypoint_speed_gov){ waypoint_speed_gov +=(int)(100.0 * dTnav); // increase at .5/ms max_speed =waypoint_speed_gov; } return max_speed;}////////////////////////////////////////////////////////////////////////////////////// Utilities//int32_t wrap_36000(int32_t ang) { if (ang> 36000) ang -=36000; if (ang <0) ang +=36000; return ang;}/* * EOS increased the precision here, even if we think that the gps is not precise enough, with 10e5 precision it has 76cm resolution * with 10e7 it's around 1 cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic, however increased * resolution also increased precision of nav calculations*/#define DIGIT_TO_VAL(_x) (_x - '0')uint32_t GPS_coord_to_degrees(char* s) { char *p, *q; uint8_t deg =0, min =0; unsigned int frac_min =0; uint8_t i; // scan for decimal point or end of field for (p =s; isdigit(*p); p++); q =s; // convert degrees while ((p - q)> 2) { if (deg) deg *=10; deg +=DIGIT_TO_VAL(*q++); } // convert minutes while (p>
 q) { if (min) min *=10; min +=DIGIT_TO_VAL(*q++); } // convert fractional minutes // expect up to four digits, result is in // ten-thousandths of a minute if (*p =='.') { q =p + 1; for (i =0; i <4; i++) { frac_min *=10; if (isdigit(*q)) frac_min +=*q++ - '0'; } } return deg * 10000000UL + (min * 1000000UL + frac_min*100UL) / 6;}// helper functions uint16_t grab_fields(char* src, uint8_t mult) { // convert string to uint16 uint8_t i; uint16_t tmp =0; for(i=0; src[i]!=0; i++) { if(src[i] =='.') { i++; if(mult==0) break; else src[i+mult] =0; } tmp *=10; if(src[i]>='0' &&src[i] <='9') tmp +=src[i]-'0'; } return tmp;}uint8_t hex_c(uint8_t n) { // convert '0'..'9','A'..'F' to 0..15 n -='0'; if(n>9) n -=7; n &=0x0F; return n;} //************************************************************************// Common GPS functions //void init_RTH() { f.GPS_mode =GPS_MODE_RTH; // Set GPS_mode to RTH f.GPS_BARO_MODE =true; GPS_hold[LAT] =GPS_coord[LAT]; //All RTH starts with a poshold GPS_hold[LON] =GPS_coord[LON]; //This allows to raise to rth altitude GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON], &GPS_hold[LAT], &GPS_hold[LON]); NAV_paused_at =0; if (GPS_conf.rth_altitude ==0) set_new_altitude(alt.EstAlt); //Return at actual altitude else { // RTH altitude is defined, but we use it only if we are below it if (alt.EstAlt =5) { GPS_home[LAT] =GPS_coord[LAT]; GPS_home[LON] =GPS_coord[LON]; GPS_calc_longitude_scaling(GPS_coord[LAT]); //need an initial value for distance and bearing calc nav_takeoff_bearing =att.heading; //save takeoff heading //TODO:Set ground altitude f.GPS_FIX_HOME =1; }}//reset navigation (stop the navigation processor, and clear nav)void GPS_reset_nav(void) { uint8_t i; for(i=0;i<2;i++) { nav[i] =0; reset_PID(&posholdPID[i]); reset_PID(&poshold_ratePID[i]); reset_PID(&navPID[i]); NAV_state =NAV_STATE_NONE; //invalidate JUMP counter jump_times =-10; //reset next step counter next_step =1; //Clear poi GPS_poi[LAT] =0; GPS_poi[LON] =0; f.GPS_head_set =0; }}//Get the relevant P I D values and set the PID controllers void GPS_set_pids(void) { posholdPID_PARAM.kP =(float)conf.pid[PIDPOS].P8/100.0; posholdPID_PARAM.kI =(float)conf.pid[PIDPOS].I8/100.0; posholdPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; poshold_ratePID_PARAM.kP =(float)conf.pid[PIDPOSR].P8/10.0; poshold_ratePID_PARAM.kI =(float)conf.pid[PIDPOSR].I8/100.0; poshold_ratePID_PARAM.kD =(float)conf.pid[PIDPOSR].D8/1000.0; poshold_ratePID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; navPID_PARAM.kP =(float)conf.pid[PIDNAVR].P8/10.0; navPID_PARAM.kI =(float)conf.pid[PIDNAVR].I8/100.0; navPID_PARAM.kD =(float)conf.pid[PIDNAVR].D8/1000.0; navPID_PARAM.Imax =POSHOLD_RATE_IMAX * 100; }//It was moved here since even i2cgps code needs itint32_t wrap_18000(int32_t ang) { if (ang> 18000) ang -=36000; if (ang <-18000) ang +=36000; return ang;}/**************************************************************************************//**************************************************************************************/...This file has been truncated, please download it to see its full contents.

Esquemas