Rev 2200 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
/*
* FollowMe.c
*
* Created on: 18.05.2012
* Author: cebra
*/
/*****************************************************************************
* Copyright (C) 2011 Christian "Cebra" Brandtner, brandtner@brandtner.net *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
* *
* *
* Credits to: *
* Holger Buss & Ingo Busker from mikrokopter.de for the MK project + SVN *
* http://www.mikrokopter.de *
* Gregor "killagreg" Stobrawa for his version of the MK code *
* Thomas Kaiser "thkais" for the original project. See *
* http://www.ft-fanpage.de/mikrokopter/ *
* http://forum.mikrokopter.de/topic-4061-1.html *
* Claas Anders "CaScAdE" Rathje for providing the font and his C-OSD code *
* http://www.mylifesucks.de/oss/c-osd/ *
* Harald Bongartz "HaraldB" for providing his Ideas and Code for usibility*
*****************************************************************************/
//############################################################################
//# HISTORY followme.c
//#
//#
//#
//#
//#
//# 22.09.2015 Starter
//# - FollowMeStep2 erweitert mit Kreisberechnung und test auf PKT
//# - PKT-Pos von lat lon auf latitude und longitude umbenannt
//#
//# 20.09.2015 Starter
//# FollowMeStep2 erweitert auf aktuelle GPS-Daten und followme_calculate_offset(...)
//#
//# 03.08.2015 Cebra
//# - add: void Debug_GPS (void) hinzugefügt zur Test der GPS Berechnung FollowMe
//#
//# 20.07.2015 CB
//# - chg: FollowMe Datensatz an NC.211 angepasst
//#
//# 27.06.2014 OG
//# - chg: Anzeige von Satfix und Satanzahl auf MINVERSX, MNORMALX
//#
//# 26.06.2014 OG
//# - chg: angepasst auf neue NMEA-Datenstruktur (gps_nmea.h)
//#
//# 24.06.2014 OG
//# - chg: FollowMe() angepasst auf geaendertes GPSMouse_ShowData()
//#
//# 22.06.2014 OG
//# - chg: FollowMe() umgestellt auf GPSMouse_ShowData() (in gps/gpsmouse.c)
//# - del: Variable CheckGPS
//#
//# 21.06.2014 OG
//# - chg: verschiedene Layoutaenderungen am Anzeigescreen und Anzeige der
//# Entfernung zwischen Kopter und Target
//# - chg: MK-Timeoutout Erkennung verbessert/angepasst
//#
//# 19.06.2014 OG
//# - erster Prototyp der Follow Me zum Kopter sendet
//# - etliche Aenderungen im Codeaufbau
//#
//# 01.06.2014 OG
//# - chg: FollowMe() - verschiedene Anzeigefunktionen auskommentiert und
//# als DEPRICATED Klassifiziert wegen Cleanup der alten OSD Screens
//# - chg: FollowMe() - Check bzgl. NC-Hardware entfernt da das bereits durch das
//# Hauptmenue erledigt wird
//#
//# 13.05.2014 OG
//# - chg: FollowMe() - Variable 'flag' auskommentiert
//# wegen Warning: variable set but not used
//# - chg: FollowMe() - Variable 'old_FCFlags' auskommentiert
//# wegen Warning: variable set but not used
//# - chg: FollowMe() - den Bereich in dem FC-Settings geladen werdeb
//# auskommentiert weil man das a) vorallem nicht benoetigt
//# und b) die load_setting() so nicht mehr existiert
//# (der Codebereich kann meines erachtens geloescht werden)
//# - del: verschiedene Verweise auf 'mk_param_struct' entfernt, weil es
//# das a) nicht mehr gibt und b) hier gar nicht benoetigt wird
//# - chg: FollowMe() - OSD_Timeout() entfernt (weil es das nicht mehr gibt)
//# und erstmal durch ein PKT_Message_P() ersetzt
//# * ACHTUNG: UNGETESTET! * (bei Bedarf anpassen, followme hat niedrige Prio)
//# - add: #include "../pkt/pkt.h"
//#
//# 05.05.2013 Cebra
//# - chg: #ifdef USE_FOLLOWME
//#
//############################################################################
#include "../cpu.h"
#include <avr/io.h>
#include <inttypes.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <string.h>
#include <stdarg.h>
#include <stdio.h>
#include "../main.h"
#ifdef USE_FOLLOWME
#include "../followme/followme.h"
#include "../osd/osd.h"
#include "../lcd/lcd.h"
#include "../timer/timer.h"
#include "../uart/usart.h"
#include "../eeprom/eeprom.h"
#include "../messages.h"
#include "../bluetooth/bluetooth.h"
#include "../setup/setup.h"
#include "../uart/uart1.h"
#include "../mk-data-structs.h"
#include "../pkt/pkt.h"
#include "../gps/gps.h"
//#include "../gps/gps_nmea.h"
#include "../avr-nmea-gps-library/nmea.h"
#include "../gps/gpsmouse.h"
//#######################################################################################################################
//--------------------
// Timings
//--------------------
//#define MK_TIMEOUT 300 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (3 sec)
#define MK_TIMEOUT 400 // MK-Verbindungsfehler wenn fuer n Zeit keine gueltigen Daten hereinkommen (4 sec)
//--------------------
#define COSD_WASFLYING 4
// global definitions and global vars
NaviData_t *naviData;
unsigned char Element;
uint16_t heading_home;
// Hier Höhenanzeigefehler Korrigieren
#define AltimeterAdjust 1.5
positionOffset followMeOffset;
// Flags
//uint8_t COSD_FLAGS2 = 0;
//
//GPS_Pos_t last5pos[7];
uint8_t FM_error = 0;
//---------------------
// Waypoint Types
// TODO OG: verschieben nach: mk-data-structs.h
//---------------------
#define POINT_TYPE_INVALID 255
#define POINT_TYPE_WP 0
#define POINT_TYPE_POI 1
//---------------------
// Status
// GPS_Pos_t
// aus MK source
//
// TODO OG: verschieben nach: mk-data-structs.h
//---------------------
#define INVALID 0x00
#define NEWDATA 0x01
#define PROCESSED 0x02
//--------------------------------------------------------------
//--------------------------------------------------------------
void FollowMe( void )
{
//uint8_t status;
GPS_Pos_t currpos;
uint8_t tmp_dat;
uint8_t redraw;
uint8_t drawmode;
uint32_t NMEA_GPGGA_counter_old; // Merker: zaehlt empfangene GPGGA-Pakete
uint32_t send_followme_counter;
int8_t ok;
int8_t xoffs;
int8_t yoffs;
Point_t FollowMe;
uint8_t mktimeout = false;
GPS_PosDev_t targetdev;
//---------------------
// 1. Daten GPS-Maus
//---------------------
ok = GPSMouse_ShowData( GPSMOUSE_SHOW_WAITSATFIX, 500 ); // 500 = 5 Sekunden Verzoegerung nach Satfix
if( ok <= 0 )
{
return; // Fehler bzgl. BT GPS-Maus -> exit
}
//---------------------
// 2. Follow Me
//---------------------
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep
lcd_cls ();
SwitchToNC();
mode = 'O';
// disable debug...
// RS232_request_mk_data (0, 'd', 0);
tmp_dat = 0;
SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
// request OSD Data from NC every 100ms
// RS232_request_mk_data (1, 'o', 100);
tmp_dat = 10;
SendOutData ('o', ADDRESS_NC, 1, &tmp_dat, 1);
//OSD_active = true; // benötigt für Navidata Ausgabe an SV2
//-------------------------
// Init: Timer & Flags
//-------------------------
timer_mk_timeout = MK_TIMEOUT;
abo_timer = ABO_TIMEOUT;
MKLiPoCells_Init();
redraw = true;
NMEA.Counter = 0;
NMEA_GPGGA_counter_old = 0;
send_followme_counter = 0;
while( (receiveNMEA) )
{
//-----------------------------------------
// Screen redraw
//-----------------------------------------
if( redraw )
{
lcd_cls();
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMe") ); // Titel: oben, mitte
lcd_line( (6*6-3), 0, (6*6-3), 11, 1); // Linie Vertikal links
lcd_line( (15*6+5), 0, (15*6+5), 11, 1); // Linie Vertikal rechts
lcd_line( 0, 12, 127, 12, 1); // Linie Horizontal
//lcd_line( 0, 39, 127, 39, 1); // Linie Horizontal Mitte
lcd_line( 66, 39, 127, 39, 1); // Linie Horizontal Mitte
lcd_rect_round( 0, 33, 66, 12, 1, R1); // Rahmen fuer "Di" (Distance)
lcdx_printf_at_P( 3, 2, MNORMAL, 3,-1, PSTR("Al:") ); // Label: "Al:"
draw_icon_mk( 1, 18);
draw_icon_target_round( 1, 50);
redraw = false;
}
//-----------------------------------------
// neue MK Daten vorhanden
//-----------------------------------------
if( rxd_buffer_locked ) // neue MK Daten
{
Decode64 ();
naviData = (NaviData_t *) pRxData;
if( mktimeout ) redraw = true;
mktimeout = false;
FM_error = 0; // noch benoetigt?
currpos.Latitude = naviData->CurrentPosition.Latitude;
currpos.Longitude = naviData->CurrentPosition.Longitude;
//----------------------------------
// speichere letzte GPS-Positionen
//----------------------------------
Config.LastLatitude = naviData->CurrentPosition.Latitude; // speichere letzte Position in Config
Config.LastLongitude = naviData->CurrentPosition.Longitude; // speichere letzte Position in Config
//----------------------------------
// LiPo Cell Check
//----------------------------------
MKLiPoCells_Check(); // ggf. Zellenzahl ermitteln
//#################
//# MK
//#################
//-----------------
// Oben: MK-Batt Level (Volt)
//-----------------
OSD_Element_BattLevel2( 0, 0, 0,0 );
//-----------------
// Oben: Batt Level Bar
//-----------------
OSD_Element_Battery_Bar( 0, 9, 30, 1, ORIENTATION_H);
//-----------------
// Oben: MK-Flugzeit
//-----------------
writex_time(16, 0, naviData->FlyingTime, MNORMAL, 2,0);
//-----------------
// Hoehe
//-----------------
xoffs = 3;
yoffs = -1;
writex_altimeter( 7, 2, naviData->Altimeter, MNORMAL, xoffs,yoffs );
//-----------------
// MK: Sat Anzahl
//-----------------
yoffs = -27;
if( naviData->SatsInUse > 5 )
drawmode = MNORMALX;
else
drawmode = MINVERSX;
writex_ndigit_number_u( 17, 5, naviData->SatsInUse, 2, 0,drawmode, 3,2+yoffs);
draw_icon_satmini( 116+3, 42+yoffs);
//-----------------
// MK: GPS
//-----------------
writex_gpspos( 3, 4, currpos.Latitude , MNORMAL,-3,-8 ); // Line 4 L: Latitude
writex_gpspos(12, 4, currpos.Longitude, MNORMAL, 1,-8 ); // Line 4 R: Longitude
// lcdx_printf_at_P( 1, 4, MNORMAL, -3,-8 , PSTR("%d"), NMEA.Latitude);
// lcdx_printf_at_P( 10, 4, MNORMAL, -1,-8 , PSTR("%d"), NMEA.Longitude);
//#################
//# DISTANCE TO TARGET
//#################
//-----------------
// Target: Distance to Target
//-----------------
xoffs = 7;
yoffs = 4;
//GPS_PosDev_t gps_Deviation( GPS_Pos_t pos1, GPS_Pos_t pos2 )
targetdev = gps_Deviation( FollowMe.Position, naviData->CurrentPosition);
lcdx_printf_at_P( 0, 4, MNORMAL, xoffs,yoffs , PSTR("Di: %3d m"), (targetdev.Distance / 10) );
//#################
//# TARGET (GPS Maus)
//#################
yoffs = 8;
//-----------------
// Send Counter
//-----------------
//lcdx_printf_at_P(4, 5, MNORMAL, 0,5, PSTR("S: %ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen
lcdx_printf_at_P( 4, 5, MNORMAL, -3,yoffs, PSTR("Tx:%5ld"), send_followme_counter ); // Anzahl gesendeter Target-Positionen
//-----------------
// Target: Fix
//-----------------
if( NMEA.SatFix == 1 || NMEA.SatFix == 2 )
drawmode = MNORMALX;
else
drawmode = MINVERSX;
lcdx_printf_at_P( 14, 5, drawmode, 1,yoffs, PSTR("F:%1d"), NMEA.SatFix ); // GPS-Maus: Satfix
//-----------------
// Target: Sat Anzahl
//-----------------
if( NMEA.SatsInUse > 5 )
drawmode = MNORMALX;
else
drawmode = MINVERSX;
writex_ndigit_number_u( 17, 5, NMEA.SatsInUse, 2, 0,drawmode, 4,yoffs);
draw_icon_satmini( 116+4, 40+yoffs);
//-----------------
// Target: Lat / Long
//-----------------
writex_gpspos( 3, 7, NMEA.Latitude , MNORMAL,-3,1 ); // GPS-Maus: Latitude
writex_gpspos(12, 7, NMEA.Longitude, MNORMAL, 1,1 ); // GPS-Maus: Longitude
rxd_buffer_locked = FALSE;
timer_mk_timeout = MK_TIMEOUT;
}
//-----------------------------------------
// if( !NMEA_receiveBT() )
// {
// receiveNMEA = false;
// }
// else
// {
// NMEA_GetNewData();
// }
// Sourcecode aus NaviCtrl/V2.10e/uart1.c
// FOLLOW_ME
// else if(CheckDelay(UART1_FollowMe_Timer) && (UART1_tx_buffer.Locked == FALSE))
// {
// if((GPSData.Status != INVALID) && (GPSData.SatFix == SATFIX_3D) && (GPSData.Flags & FLAG_GPSFIXOK) && (GPSData.NumOfSats >= 4))
// {
// TransmitAlsoToFC = 1;
// // update FollowMe content
// FollowMe.Position.Longitude = GPSData.Position.Longitude;
// FollowMe.Position.Latitude = GPSData.Position.Latitude;
// FollowMe.Position.Status = NEWDATA;
// FollowMe.Position.Altitude = 1;
// // 0 -> no Orientation
// // 1-360 -> CompassCourse Setpoint
// // -1 -> points to WP1 -> itself
// FollowMe.Heading = -1;
// FollowMe.ToleranceRadius = 1;
// FollowMe.HoldTime = 60;
// FollowMe.Event_Flag = 1;
// FollowMe.Index = 1; // 0 = Delete List, 1 place at first entry in the list
// FollowMe.Type = POINT_TYPE_WP;
// FollowMe.WP_EventChannelValue = 100; // set servo value
// FollowMe.AltitudeRate = 0; // do not change height
// FollowMe.Speed = 0; // rate to change the Position (0 = max)
// FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic)
// FollowMe.Name[0] = 'F'; // Name of that point (ASCII)
// FollowMe.Name[1] = 'O'; // Name of that point (ASCII)
// FollowMe.Name[2] = 'L'; // Name of that point (ASCII)
// FollowMe.Name[3] = 'L'; // Name of that point (ASCII)
// FollowMe.reserve[0] = 0; // reserve
// FollowMe.reserve[1] = 0; // reserve
// MKProtocol_CreateSerialFrame(&UART1_tx_buffer, 's', NC_ADDRESS, 1, (u8 *)&FollowMe, sizeof(FollowMe));
// }
// UART1_FollowMe_Timer = SetDelay(FOLLOW_ME_INTERVAL); // set new update time
// }
//
//-----------------------------------------
// neue NMEA Daten?
//-----------------------------------------
if(NMEA_isdataready() && receiveNMEA)
{
if( NMEA.Counter > NMEA_GPGGA_counter_old )
{
if( (NMEA.SatsInUse > 5) && (NMEA.SatFix == 1 || NMEA.SatFix == 2) )
{
//Config.FM_Refresh
// FollowMeStep2
followMeOffset.latitude = 2000;
followMeOffset.longitude = 2000;
followme_add_offset(&NMEA, &NMEA, &followMeOffset);
FollowMe.Position.Status = NEWDATA;
FollowMe.Position.Longitude = NMEA.Longitude;
FollowMe.Position.Latitude = NMEA.Latitude;
FollowMe.Position.Altitude = 1; // 20.7.2015 CB
// FollowMe.Position.Altitude = NMEA.Altitude; // ist das wirklich ok? NEIN C.B.
FollowMe.Heading = -1; // invalid heading
FollowMe.ToleranceRadius = Config.FM_Radius; // 5 meter default
FollowMe.HoldTime = 60; // ????? go home after 60s without any update ??????
// FollowMe.Event_Flag = 0; // no event
FollowMe.Event_Flag = 1; // 20.7.2015 CB
FollowMe.Index = 1; // 2st wp, 0 = Delete List, 1 place at first entry in the list
FollowMe.Type = POINT_TYPE_WP; // Typ des Wegpunktes
FollowMe.Name[0] = 'F'; // Name des Wegpunktes (ASCII)
FollowMe.Name[1] = 'O';
FollowMe.Name[2] = 'L';
FollowMe.Name[3] = 'L';
// FollowMe.WP_EventChannelValue = 0; // Will be transferred to the FC and can be used as Poti value there
FollowMe.WP_EventChannelValue = 100; // set servo value 20.7.2015
FollowMe.AltitudeRate = 0; // rate to change the Aetpoint
FollowMe.Speed = Config.FM_Speed; // rate to change the Position
FollowMe.CamAngle = 255; // Camera servo angle in degree (255 -> POI-Automatic)
FollowMe.reserve[0] = 0; // reserve
FollowMe.reserve[1] = 0; // reserve
SendOutData( 's', ADDRESS_NC, 1, &FollowMe, sizeof(FollowMe) ); //'s' = target Position 'w' = Waypoint
send_followme_counter++;
//void SendOutData(uint8_t cmd, uint8_t addr, uint8_t numofbuffers, ...) // uint8_t *pdata, uint8_t len, ...
// SendOutData ('d', ADDRESS_ANY, 1, &tmp_dat, 1);
//SendOutData( 's', NC_ADDRESS, 1, (uint8_t *)&FollowMe, sizeof(FollowMe)); //'s' = target Position 'w' = Waypoint
}
NMEA_GPGGA_counter_old = NMEA.Counter;
}
}
//-----------------------------------------
// TASTEN
//-----------------------------------------
if( get_key_press(1 << KEY_ESC) )
{
break;
}
if( get_key_press(1 << KEY_ENTER) )
{
break;
}
//-----------------------------------------
//-----------------------------------------
if( !abo_timer )
{
// renew abo every 3 sec
// request OSD Data from NC every 100ms
// RS232_request_mk_data (1, 'o', 100);
tmp_dat = 10;
SendOutData ( 'o', ADDRESS_NC, 1, &tmp_dat, 1);
abo_timer = ABO_TIMEOUT;
}
//--------------------------
// Daten Timeout vom MK?
//--------------------------
if( timer_mk_timeout == 0 )
{
if( !mktimeout ) OSD_MK_ShowTimeout(); // nur anzeigen wenn noch nicht im mktimeout-Modus
set_beep ( 200, 0x0080, BeepNormal); // Beep
mktimeout = true;
timer_mk_timeout = MK_TIMEOUT;
//OSDScreenRefresh = OSD_SCREEN_REDRAW;
// OSD_MK_Connect( MK_CONNECT );
}
} // end: while( (receiveNMEA) );
//---------------------
// BEENDEN
//---------------------
OSD_active = false;
//---------------------
// GPS beenden
//---------------------
GPSMouse_Disconnect();
}
//
#ifdef USE_FOLLOWME_STEP2
void Debug_GPS (void)
{
uint8_t redraw = true;
nmeaPOS NMEApos;
nmeaPOS NMEATarget;
set_beep( 25, 0xffff, BeepNormal ); // kurzer Bestaetigungs-Beep
#ifdef ONLINE
int retcode = GPSMouse_Connect(); // Abfrage der GPS-Daten zum testen -> Quick an Dirty ;-)
if( retcode <= 0 )
{
return;
}
#endif
#ifdef DEBUG
// NMEApos.lat = 520000000;
// NMEApos.lon = 0;
// Config.FM_Azimuth = 90;
// Config.FM_Distance = 10000;
#endif
while( true )
{
NMEApos.latitude = NMEA.Latitude;
NMEApos.longitude = NMEA.Longitude;
if( redraw )
{
lcd_cls();
lcdx_printf_center_P( 0, MNORMAL, 1,0, PSTR("FollowMeStep2") );
lcdx_printf_center_P( 1, MNORMAL, 1,0, PSTR(" Source Lat/Lon") );
lcdx_printf_center_P( 3, MNORMAL, 1,0, PSTR(" Target Lat/Lon") );
redraw = false;
}
writex_gpspos( 1, 2, NMEApos.latitude, MNORMAL, 0,0 ); // GPS-Maus: Latitude
writex_gpspos(10, 2, NMEApos.longitude, MNORMAL, 0,0 ); // GPS-Maus: Longitude
followme_calculate_offset(Config.FM_Distance, Config.FM_Azimuth, &followMeOffset);
#ifdef DEBUG
writex_gpspos( 1, 6, (int32_t)Config.FM_Azimuth*100, MNORMAL, 0, 0 );
writex_gpspos( 10, 6, (int32_t)Config.FM_Distance*100, MNORMAL, 0, 0 );
writex_gpspos( 1, 7, (int32_t)followMeOffset.latitude*100, MNORMAL, 0, 0 );
writex_gpspos( 10, 7, (int32_t)followMeOffset.longitude*100, MNORMAL, 0, 0 );
#endif
followme_add_offset(&NMEApos, &NMEATarget, &followMeOffset);
writex_gpspos( 1, 4, (int32_t)NMEATarget.latitude, MNORMAL, 0, 0 ); // Ziel Latitude
writex_gpspos(10, 4, (int32_t)NMEATarget.longitude, MNORMAL, 0, 0 ); // Ziel Longitude
// Tasten
if( get_key_press(1 << KEY_ESC) )
{
#ifdef ONLINE
GPSMouse_Disconnect();
#endif
break;
}
if( get_key_press(1 << KEY_ENTER) )
{
redraw = true;
}
if( get_key_press(1 << KEY_MINUS) )
{
Config.FM_Azimuth -= 10;
redraw = true;
}
if( get_key_press(1 << KEY_PLUS) )
{
Config.FM_Azimuth += 10;
redraw = true;
}
}
}
#endif // FOLLOW_ME_STEP2
#endif // #ifdef USE_FOLLOWME