Arduino Quadcopter

Sobre este projeto

Não é apenas um quadricóptero ... é uma máquina de código aberto!

Etapa 1:Projeto do controlador de voo

• D3 <

• D9 <

• D10 <

• D11 <

• TX <

• RX <

• A4 <

• A5 <

• D8 <

• D2 <

• D4 <

• D5 <

• D6 <

• D7 <

Etapa 2:Construindo uma Estrutura

Etapa 3:Anexando o controlador de vôo na estrutura

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 // Protótipos de funções para outras funções de GPS // Talvez possam ir para o arquivo gps.h, no entanto, local para o gps.cpp estático vazio GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * rolamento); vazio estático GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2 , uint32_t * dist); estático void GPS_calc_velocity (void); estático void GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); estático vazio GPS_calc_location_error (int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); estático voidlow_calc_location_poshold _speeds_calc_poshold (uspeed_poshold_calc16) static_calc_poshold; ); estático vazio GPS_calc_nav_rate (uint16_t max_speed); int32_t wrap_18000 (int32_t ang); bool estático check_missed_wp (vazio); vazio GPS_calc_longitude_scaling (int32_t lat); estático vazio GPS_update_crosstrack (void) p_36000 (int32_t ang); // Filtro Leadig - TODO:reescrever para C normal em vez de C ++ // Configurar lag # se definido (UBLOX) || definido (MTK_BINARY19) #define GPS_LAG 0.5f // UBLOX GPS tem um atraso menor que MTK e outro # else # define GPS_LAG 1.0f // Assumimos que MTK GPS tem um atraso de 1 seg # endif static int32_t GPS_coord_lead [2]; // Lead filtrada classe de coordenadas gps LeadFilter {public:LeadFilter ():_last_velocity (0) {} // configurar valores de rádio mínimo e máximo em CLI int32_t get_position (int32_t pos, int16_t vel, float lag_in_seconds =1.0); void clear () {_last_velocity =0; } private:int16_t _last_velocity;}; int32_t LeadFilter ::get_position (int32_t pos, int16_t vel, float lag_in_seconds) {int16_t accel_contribution =(- _last_velocity) * lag_ vel_seconds * lag_in_seconds; int16_t vel_contribution =vel * lag_in_seconds; // armazena velocidade para a próxima iteração _last_velocity =vel; retornar pos + vel_contribution + accel_contribution;} LeadFilter xLeadFilter; // Filtro de longo atraso do GPS LeadFilter yLeadFilter; // Tipo de filtro Lat GPS lag struct PID_PARAM_ {float kP; float kI; float kD; float Imax; } PID_PARAM; PID_PARAM posholdPID_PARAM; PID_PARAM poshold_ratePID_PARAM; PID_PARAM navPID_PARAM; typedef struct PID_ {integrador flutuante; // valor do integrador int32_t last_input; // última entrada para derivada float lastderivative; // última derivada para saída flutuante do filtro passa-baixa; derivada flutuante;} PID; PID posholdPID [2]; PID poshold_ratePID [2]; PID navPID [2]; int32_t get_P (erro int32_t, struct PID_PARAM_ * pid) {return (float) erro * pid-> kP;} int32_t get_I (erro int32_t, float * dt, struct PID_ * pid, struct PID_PARAM_ * pid_param) {pid-> integrador + =((float) erro * pid_param-> kI) * * dt; pid-> integrador =restrição (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 em milissegundos pid-> derivative =(input - pid-> last_input) / * dt; /// Freqüência de corte do filtro passa-baixa para cálculo derivativo. filtro flutuante =7,9577e-3; // Definir como "1 / (2 * PI * f_cut)"; // Exemplos para _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 passa-baixo discreto, corta o // ruído de alta frequência que pode deixar o controlador louco pid-> derivado =pid-> último derivado + (* dt / (filtro + * dt)) * (pid-> derivado - pid-> último derivado); // atualiza o estado pid-> last_input =input; pid-> lastderivative =pid-> derivative; // adiciona o 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 land (extern) static uint32_t land_settle_timer; uint8_t GPS_Frame; // um GPS_Frame válido foi detectado e os dados estão prontos para nav computationstatic float dTnav; // Delta Time em milissegundos para cálculos de navegação, atualizado com cada boa leitura estática de GPS int16_t actual_speed [2] ={0,0}; static float GPS_scaleLonDown; // isso é usado para compensar a redução da longitude conforme avançamos em direção aos pólos // A diferença entre a taxa de deslocamento desejada e a taxa real de deslocamento // atualizada após a leitura do GPS - 5-10hzstatic int16_t rate_error [2]; estático int32_t erro [2]; estático int32_t GPS_WP [2]; // Atualmente usado WPstatic int32_t GPS_FROM [2]; // o ponto de passagem permeável para rastreamento preciso followingint32_t target_bearing; // Este é o ângulo do helicóptero ao local "next_WP" em graus * 100static int32_t original_target_bearing; // deg * 100, O ângulo original para o next_WP quando o next_WP foi definido, Também usado para verificar quando passamos um WPstatic int16_t crosstrack_error; // A quantidade de correção de ângulo aplicada a target_bearing para trazer o helicóptero de volta ao seu pathuint32_t wp_distance; // distância entre o plano e next_WP em cmstatic uint16_t waypoint_speed_gov; // usado para velocidade lenta, acelera ao iniciar a navegação; //////////////////////////////////////// //////////////////////////////////////////////////// Média móvel filtrar variáveis ​​// # define GPS_FILTER_VECTOR_LENGTH 5static uint8_t GPS_filter_index =0; static int32_t GPS_filter [2] [GPS_FILTER_VECTOR_LENGTH]; estático int32_t GPS_filter_sum [2]; estático int32_t GPS_filter [2] [GPS_FILTER_VECTOR_LENGTH]; estático int32_t GPS_filter_sum [2];; // o grau de lat lon sem decimais (lat / 10.000.000) estático uint16_t fração3 [2]; estático int16_t nav_takeoff_bearing; // salva o rumo na takeof (1deg =1) usado para girar para a direção da decolagem quando chega em casa // Processador de navegação principal e motor de estado // TODO:adiciona estados de processamento para facilitar a carga de processamento uint8_t GPS_Compute (void) {eixo de caractere não assinado; uint32_t dist; // variável temporária para armazenar dist no copter int32_t dir; // variável temporária para armazenar dir no copter static uint32_t nav_loopTimer; // verifique se temos um frame válido, senão retorna imediatamente if (GPS_Frame ==0) return 0; senão GPS_Frame =0; // verifique a posição inicial e defina se não foi definida if (f.GPS_FIX &&GPS_numSat> =5) {#if! definido (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 filtro de média móvel aos dados GPS if (GPS_conf.filtering) {GPS_filter_index =(GPS_filter_index + 1)% GPS_FILTER_VECTOR_LENGTH; para (eixo =0; eixo <2; eixo ++) {GPS_read [eixo] =GPS_coord [eixo]; // os dados não filtrados mais recentes estão em GPS_latitude e GPS_longitude GPS_degree [axis] =GPS_read [axis] / 10000000; // obtenha o grau para garantir que a soma se ajuste ao int32_t // Quão próximos estamos de uma linha de grau? são os primeiros três dígitos das frações de grau // depois usamos para Verificar se estamos próximos de uma linha de graus, se sim, desabilitar a média, fração3 [eixo] =(GPS_read [eixo] - GPS_degree [eixo] * 10000000 ) / 10000; GPS_filter_sum [eixo] - =GPS_filter [eixo] [GPS_filter_index]; GPS_filter [eixo] [GPS_filter_index] =GPS_read [eixo] - (GPS_degree [eixo] * 10000000); GPS_filter_sum [eixo] + =GPS_filter [eixo] [GPS_filter_index]; GPS_filtered [eixo] =GPS_filter_sum [eixo] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree [eixo] * 10000000); if (NAV_state ==NAV_STATE_HOLD_INFINIT || NAV_state ==NAV_STATE_HOLD_TIMED) {// usamos a média gps apenas no modo poshold ... if (fração3 [eixo]> 1 &&fração3 [eixo] <999) GPS_coord [eixo] =GPS_filtrado [ eixo]; }}} // cálculo dTnav // Tempo para calcular a velocidade x, y e pids de navegação dTnav =(float) (millis () - nav_loopTimer) / 1000.0; nav_loopTimer =millis (); // previne o runup de GPS ruim dTnav =min (dTnav, 1.0); // calcula a distância e a orientação para gui e outras coisas continuamente - Da casa para o 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) {// Se não tivermos uma configuração inicial, não exibirá nada GPS_distanceToHome =0; GPS_directionToHome =0; } // Verifique a configuração da cerca e execute RTH se necessário // TODO:autolanding if ((GPS_conf.fence> 0) &&(GPS_conf.fence  5000) NAV_state =NAV_STATE_LAND_START_DESCENT; pausa; case NAV_STATE_LAND_START_DESCENT:GPS_calc_poshold (); // Pouse na posição hold f.THROTTLE_IGNORED =1; // Ignora o Throtte stick input f.GPS_BARO_MODE =1; // Assuma o controle do modo BARO land_detect =0; // Reinicia o detector de terra f.LAND_COMPLETED =0; f.LAND_IN_PROGRESS =1; // Sinalizar processo de terra NAV_state =NAV_STATE_LAND_IN_PROGRESS; pausa; case NAV_STATE_LAND_IN_PROGRESS:GPS_calc_poshold (); check_land (); // Chama o detector terrestre if (f.LAND_COMPLETED) {nav_timer_stop =millis () + 5000; NAV_state =NAV_STATE_LANDED; } pausa; case NAV_STATE_LANDED:// Desarmar se o stick do THROTTLE estiver no mínimo ou 5 segundos após o solo ser detectado se (rcData [THROTTLE]  0) {// se zero não for atingido, faça um salto next_step =mission_step.parameter1; NAV_state =NAV_STATE_PROCESS_NEXT; jump_times--; } pausa; case NAV_STATE_PROCESS_NEXT:// Processando a próxima etapa da missão NAV_error =NAV_ERROR_NONE; if (! recallWP (next_step)) {abort_mission (NAV_ERROR_WP_CRC); } else {switch (mission_step.action) {// Waypoiny e os comandos hold todos começam com um status de rota que inclui o comando LAND também 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); else NAV_state =NAV_STATE_WP_ENROUTE; GPS_prev [LAT] =mission_step.pos [LAT]; // Salvar coordenadas wp para rota precisa calc GPS_prev [LON] =mission_step.pos [LON]; pausa; case MISSION_RTH:f.GPS_head_set =0; if (GPS_conf.rth_altitude ==0 &&mission_step.altitude ==0) // se config e mission_step alt for zero set_new_altitude (alt.EstAlt); // RTH retorna na altitude real else {uint32_t rth_alt; if (mission_step.altitude ==0) rth_alt =GPS_conf.rth_altitude * 100; // altitude na etapa da missão tem prioridade 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); Retorna; } // Começamos na altitude da localização atual e mudamos gradualmente alt alt_to_hold =alt.EstAlt; // para calcular o tempo delta alt_change_timer =millis (); // salvar a altitude alvo target_altitude =_new_alt; // redefine nosso integrador de altitude alt_change =0; // salve a altitude original original_altitude =alt.EstAlt; // para decidir se atingimos a altitude alvo if (target_altitude> original_altitude) {// estamos abaixo, subindo alt_change_flag =ASCENDING; } else if (target_altitude  =target_altitude) alt_change_flag =REACHED_ALT; // não devemos comandar além de nosso alvo if (alt_to_hold> =target_altitude) return target_altitude; } else if (alt_change_flag ==DESCENDING) {// estamos acima, indo para baixo if (alt.EstAlt <=target_altitude) alt_change_flag =REACHED_ALT; // não devemos comandar além de nosso alvo if (alt_to_hold <=target_altitude) return target_altitude; } // se alcançamos nossa altitude alvo, retorna o alt alvo if (alt_change_flag ==REACHED_ALT) return target_altitude; int32_t diff =abs (alt_to_hold - target_altitude); // escala é como geramos uma taxa desejada a partir do tempo decorrido // uma escala menor significa taxas mais rápidas int8_t _scale =4; if (alt_to_hold > 4 =64 cm / s descida por padrão int32_t change =(millis () - alt_change_timer)>> _scale; if (alt_change_flag ==ASCENDING) {alt_change + =alterar; } else {alt_change - =alterar; } // para gerar tempo delta alt_change_timer =millis (); return original_altitude + alt_change;} //////////////////////////////////////////////// ////////////////////////////////////////////// Funções de navegação GPS baseadas em PID // Autor :EOSBandi // Baseado no código e nas ideias da equipe Arducopter:Jason Short, Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen // Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni // restrição original não funciona com as variáveisint16_t constrain_int16 (int16_t amt, int16_t baixo, int16_t alto) {return ((amt) <(baixo)? (baixo):((amt)> (alto)? (alto) :( amt))); } ///////////////////////////////////////////////////// ///////////////////////////////////////// Isto é usado para compensar a redução da longitude conforme avançamos para o pólos // Não há problema em calcular isso uma vez por configuração de waypoint, já que muda um pouco dentro do alcance de um multicóptero // void GPS_calc_longitude_scaling (int32_t lat) {GPS_scaleLonDown =cos (lat * 1.0e-7f * 0.01745329251f);} / //////////////////////////////////////////////////////// /////////////////////////// ////////// Define o ponto de passagem para navegar, redefinir as variáveis ​​necessárias e calcular os valores iniciais // 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; original_target_bearing =target_bearing;} ///////////////////////////////////////////////// ///////////////////////////////////////////// Verifique se perdemos o destino de alguma forma // bool estático check_missed_wp (void) {int32_t temp; temp =target_bearing - original_target_bearing; temp =wrap_18000 (temp); retorno (abs (temp)> 10.000); // ultrapassamos o ponto de passagem em 100 graus} ////////////////////////////////////////// //////////////////////////////////////////////////// Obtenha distância entre dois pontos em cm // obter o rumo de pos1 para pos2, retorna um 1deg =100 precisionvoid GPS_bearing (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * rumo) {int32_t off_x =* lon2 - * lon1; int32_t off_y =(* lat2 - * lat1) / GPS_scaleLonDown; * rolamento =9000 + atan2 (-off_y, off_x) * 5729.57795f; // Converte os redianos de saída para 100xdeg if (* rolamento <0) * rolamento + =36000;} void GPS_distance_cm (int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, uint32_t * dist) {float dLat =( float) (* lat2 - * lat1); // diferença de latitude em 1/10 000 000 graus float dLon =(float) (* lon2 - * lon1) * GPS_scaleLonDown; // x * dist =sqrt (sq (dLat) + sq (dLon)) * 1.11318845f;} // ************************* *************************************************** ***************************** // calc_velocity_and_filtered_position - velocidade em direções lon e lat calculadas a partir da posição GPS // e dados do acelerômetro // lon_speed expresso em cm / s. números positivos significam mover para o leste // lat_speed expresso em cm / s. números positivos ao mover para o norte // Nota:usamos localizações GPS diretamente para calcular a velocidade em vez de pedir velocidade ao GPS porque // isso é mais preciso abaixo de 1,5 m / s // Nota:embora as posições sejam projetadas usando um filtro de chumbo, as velocidades são calculadas // a partir das localizações gps inalteradas. Não queremos que o ruído do nosso filtro principal afete a velocidade // *************************************** *************************************************** ***************** 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; velocidade_real [_X] =(flutuante) (GPS_coord [LON] - último [LON]) * GPS_scaleLonDown * tmp; velocidade_real [_Y] =(flutuante) (GPS_coord [LAT] - último [LAT]) * tmp; // TODO:Verificar mudanças de velocidade não realistas e navegação de sinal sobre possível degradação do sinal gps if (! GPS_conf.lead_filter) {actual_speed [_X] =(actual_speed [_X] + speed_old [_X]) / 2; velocidade_real [_Y] =(velocidade_real [_Y] + velocidade_antiga [_Y]) / 2; velocidade_antiga [_X] =velocidade_real [_X]; velocidade_antiga [_Y] =velocidade_real [_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], velocidade_real [_Y], GPS_LAG); }} //////////////////////////////////////////////////// ////////////////////////////////////////// Calcule um erro de localização entre duas coordenadas GPS // Porque nós estamos usando lat e lon para fazer nossos erros de distância, aqui está um gráfico rápido:// 100 =1m // 1000 =11m =36 pés // 1800 =19,80m =60 pés // 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) {erro [LON] =(float) (* target_lng - * gps_lng) * GPS_scaleLonDown; // Erro X erro [LAT] =* target_lat - * gps_lat; // Y Error} /////////////////////////////////////////////// ///////////////////////////////////////////// Calcule nav_lat e nav_lon a partir de x e y erro e a velocidade //// TODO:verifique se a restrição de velocidade alvo poshold pode ser aumentada para mais rápida poshold lockstatic void GPS_calc_poshold (void) {int32_t d; int32_t target_speed; eixo uint8_t; para (eixo =0; eixo <2; eixo ++) {target_speed =get_P (erro [eixo], &posholdPID_PARAM); // calcula a velocidade desejada a partir do erro lat / lon target_speed =constrain (target_speed, -100,100); // Limita a velocidade alvo no modo poshold a 1m / s ajuda a evitar fugas. Rate_error [axis] =target_speed - actual_speed [axis]; // calcula o erro de velocidade nav [eixo] =get_P (rate_error [eixo], &poshold_ratePID_PARAM) + get_I (rate_error [eixo] + erro [eixo], &dTnav, &poshold_ratePID [eixo], &poshold_ratePID_PARAM); d =get_D (erro [eixo], &dTnav, &poshold_ratePID [eixo], &poshold_ratePID_PARAM); d =constrangimento (d, -2000, 2000); // livrar-se do ruído if (abs (velocidade_real [eixo]) <50) d =0; nav [eixo] + =d; // nav [eixo] =restrição (nav [eixo], -NAV_BANK_MAX, NAV_BANK_MAX); nav [eixo] =constrain_int16 (nav [eixo], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); navPID [eixo] .integrator =poshold_ratePID [eixo] .integrador; }} //////////////////////////////////////////////////// ////////////////////////////////////////// Calcule o nav_lat e nav_lon desejados para voos de longa distância, como RTH e WP // void estático GPS_calc_nav_rate (uint16_t max_speed) {float trig [2]; int32_t target_speed [2]; int32_t tilt; eixo uint8_t; GPS_update_crosstrack (); int16_t cross_speed =crosstrack_error * (GPS_conf.crosstrack_gain / 100.0); // verifique se está tudo bem? cross_speed =constrain (cross_speed, -200,200); cross_speed =-cross_speed; float temp =(9000l - target_bearing) * RADX100; trig [_X] =cos (temp); trig [_Y] =sin (temp); target_speed [_X] =max_speed * trig [_X] - cross_speed * trig [_Y]; target_speed [_Y] =cross_speed * trig [_X] + max_speed * trig [_Y]; para (eixo =0; eixo <2; eixo ++) {taxa_error [eixo] =velocidade_alvo [eixo] - velocidade_real [eixo]; taxa_error [eixo] =restrição (taxa_error [eixo], - 1000,1000); nav [eixo] =get_P (taxa_error [eixo], &navPID_PARAM) + get_I (taxa_error [eixo], &dTnav, &navPID [eixo], &navPID_PARAM) + get_D (taxa_error [eixo], &dTnav, &navPID [eixo], &navPID_PARAM); // nav [eixo] =restrição (nav [eixo], - NAV_BANK_MAX, NAV_BANK_MAX); nav [eixo] =constrain_int16 (nav [eixo], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max); poshold_ratePID [eixo] .integrator =navPID [eixo] .integrador; }} static void GPS_update_crosstrack (void) {// Crosstrack Error // ---------------- // Se estivermos muito longe ou muito perto, não rastreamos o float seguinte temp =(target_bearing - original_target_bearing) * RADX100; crosstrack_error =sin (temp) * wp_distance; // Metros que estamos fora da linha} /////////////////////////////////////////// /////////////////////////////////////////////////// Determine a velocidade desejada ao navegar em direção a um ponto de passagem, implemente também um aumento lento // de velocidade ao iniciar uma navegação //// |  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