WebSVN - NaviCtrl - Diff - Rev 515 and 516



// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand 
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang

// +  Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.

// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>      
#include <string.h>
#include "91x_lib.h"
#include "waypoints.h"
#include "uart1.h"
#include "fat16.h"

 

u8 WPActive = FALSE;
 
u8 PointList_Init(void)
{

        return PointList_Clear();
}
 
u8 PointList_Clear(void)
{
        u8 i;
        WPIndex = 0;    // real list position are 1 ,2, 3 ...
        POIIndex = 0;   // real list position are 1 ,2, 3 ...
        WPCount = 0;    // no waypoints
    POICount = 0;
        PointCount = 0; // no contents

                PointList[i].Speed = 0;
                PointList[i].CamAngle = 0;
                PointList[i].Name[0] = 0;
        }
        ClearWLP_Name();
        return TRUE;           
}

 
u8 PointList_GetCount(void)
{

 
                                        case POINT_TYPE_WP:
                                                WPCount++;
                                                PointCount++;
                                                break;
                                       
                                        case POINT_TYPE_POI:
                                                POICount++;
                                                PointCount++;
                                                break;
                                }
                                break;
                               
                        case POINT_TYPE_WP: // was a waypoint
                                switch(pPoint->Type)
                                {
                                        case POINT_TYPE_INVALID:

 
                                        default:
                                        case POINT_TYPE_WP:
                                                //nothing to do
                                                break;
                                       
                                        case POINT_TYPE_POI:
                                                POICount++;
                                                WPCount--;
                                                break;
                                }
                                break;
                               
                        case POINT_TYPE_POI: // was a poi
                                switch(pPoint->Type)
                                {
                                        case POINT_TYPE_INVALID:

 
                                        case POINT_TYPE_WP:
                                                WPCount++;
                                                POICount--;
                                                break;
                                       
                                        case POINT_TYPE_POI:
                                        default:
                                                // nothing to do
                                                break;
                                }
                                break;         
                }
                memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry                                                                                
                NaviData.WaypointNumber = WPCount;
                return pPoint->Index;
        }
        else return(0);

        WPIndex = 0; // set list position invalid

 

        if(WPActive == FALSE) return(NULL);
 
        POIIndex = 0; // set invalid POI
        if(PointCount > 0)
        {
                // search for first wp in list
                for(i = 0; i <MAX_LIST_LEN; i++)

                if(WPIndex) // found a WP in the list
                {
                        NaviData.WaypointIndex = 1;
                        // update index to POI
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
                        else POIIndex = 0;                     
                }
                else // some points in the list but no WP found
                {
                        NaviData.WaypointIndex = 0;
                        //Check for an existing POI

                        }
                }
        }
        else // no point in the list
        {
                POIIndex = 0;
                NaviData.WaypointIndex = 0;    
        }

 
        if(WPIndex) return(&(PointList[WPIndex-1]));
        else return(NULL);

}
 
// returns the last waypoint
Point_t* PointList_WPEnd(void)
{
       
        u8 i;

        WPIndex = 0; // set list position invalid

        {
                // search backward!
                for(i = 1; i <= MAX_LIST_LEN; i++)
                {
                        if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
                        {      
                                WPIndex = MAX_LIST_LEN - i + 1;
                                break;
                        }
                }
                if(WPIndex) // found a WP within the list
                {
                        NaviData.WaypointIndex = WPCount;
                        if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
                        else POIIndex = 0;     
                }
                else // list contains some points but no WP in the list
                {
                        // search backward for a POI!
                        for(i = 1; i <= MAX_LIST_LEN; i++)
                        {
                                if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
                                {      
                                        POIIndex = MAX_LIST_LEN - i + 1;
                                        break;
                                }
                        }
                        NaviData.WaypointIndex = 0;    
                }
        }
        else // no point in the list
        {
                POIIndex = 0;

// returns a pointer to the next waypoint or NULL if the end of the list has been reached
Point_t* PointList_WPNext(void)
{
        u8 wp_found = 0;
        if(WPActive == FALSE) return(NULL);
               
        if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
        {
                u8 i;
                for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
                {

                NaviData.WaypointIndex++;
                if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
                else POIIndex = 0;
                return(&(PointList[WPIndex-1]));        // return pointer to this waypoint
        }
        else
        {  // no next wp found
                NaviData.WaypointIndex = 0;    
                POIIndex = 0;
                return(NULL);
        }
}      

 
void PointList_WPActive(u8 set)
{
        if(set)
        {      
                WPActive = TRUE;
                PointList_WPBegin(); // updates POI index
        }
        else
        {
                WPActive = FALSE;
                POIIndex = 0;  // disable POI also
        }
}
 
Point_t* PointList_GetPOI(void)
{
        return PointList_GetAt(POIIndex);      

}
 

 
        if(Fat16_IsValid())
        {       // check if wpl file is existing
                if(fexist_(filename))
                {       //file is existent
                        if(!(overwride))
                        {
                                UART1_PutString("Error: file exist!\r\n");
                                return(WPL_FILEEXIST);
                        }      
                }
                fp = fopen_(filename, 'w');             // try to open the file
                if(fp == NULL)
                {
                        UART1_PutString("ERROR: Creating waypoint file!\r\n");
                        return(retval);
                }
                // Create general section and key entries
                fputs_("[General]\r\n", fp);
                sprintf(wpline, "Name=%s\r\n",  listname);
                fputs_(wpline, fp);
                sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
                fputs_(wpline, fp);
                sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
                fputs_(wpline, fp);
                // dump all points if existent

                                // write cam angle in degree (255 -> POI-Automatic)
                                sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
                                fputs_(wpline, fp);
                                // write point type
                                sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
                                fputs_(wpline, fp);    
                                // write prefix 
                                sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
                                fputs_(wpline, fp);
                        } // EOF loop over all points
                } // EOF if(PointCount)
                if(EOF == fclose_(fp))

                }
                else
                {
                        UART1_PutString("ok\r\n");
                        retval = WPL_OK;
                }                                
        } // EOF if(Fat16_IsValid())
        else
        {
                UART1_PutString("no file system found!\r\n");
                retval = WPL_NO_SDCARD_FOUND;
        }
        return(retval);

u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen)
{
        File_t *fp;
        s8 wpline[LINE_MAX];
        u8 retval = WPL_ERROR;
       
        s8 *name, *value;
        u8 i;

 
        u8 IsGeneralSection = 0;

 
        // clear point list first

        PointList_Clear();
 
        UART1_PutString("\n\r Read ");

        UART1_PutString(filename);
        UART1_PutString("...");
 

                                                        else if(strcmp(name, "TYPE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Type = (u8)atoi(value);
                                                                if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--;  // index shift
                                                                else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
                                                       
                                                                switch(PointList[IsPointSection-1].Type)
                                                                {
                                                                        case POINT_TYPE_WP:
                                                                                // this works only if altitude key is set before point type key in WPL file     !!
                                                                                PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs 
                                                                                WPCount++;
                                                                                break;

 
                                                                        case POINT_TYPE_POI:

                                                        else
                                                        {
                                                                UART1_PutString("Unknown key: ");
                                                                UART1_PutString(name);
                                                                UART1_PutString("\r\n");
                                                        }      
                                                } // EOF point section
                                                else if(IsGeneralSection)
                                                {
                                                        if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
                                                        {      
                                                                WPNumber = (u8)atoi(value);
                                                                if(!WPNumber) // no waypoints in file
                                                                {
                                                                        return(WPL_NO_WAYPOINTS); // we are done here   
                                                                }
                                                                else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
                                                                {
                                                                        UART1_PutString("To many points!");
                                                                        return(WPL_ERROR);
                                                                }
                                                        }
                                                        else if (strcmp(name, "FILEVERSION") == 0)
                                                        {
                                                                if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
                                                                {
                                                                        PointList_Clear();
                                                                        UART1_PutString("Bad file version!\r\n");
                                                                        return(WPL_ERROR);     
                                                                }
                                                        }
                                                        else if (strcmp(name, "NAME") == 0)
                                                        {
                                                                if(listname)

                                } // EOF key entry line
                        } // valid line
                } // EOF loop over all lines
                fclose_(fp);
                NaviData.WaypointNumber = WPCount;
                retval = WPL_OK;        
                UART1_PutString("ok\r\n");                               
        } // EOF if(Fat16_IsValid())
        else
        {
                UART1_PutString("no file system found!\r\n");
                retval = WPL_NO_SDCARD_FOUND;
        }      
        return(retval);
}

 
// load actual point list from SD card
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
{

        u8 filename[30];       

 
        pWPL_Store->Name[0] = 0; // clear current list name
 
        // user absolute path, i.e. leading /
        if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
        {
                sprintf(filename, "/default.wpl");     
        }
        else

 
// save actual point list to SD card
u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
{
        u8 filename[30];
       
       
        if(PointCount == 0) return(WPL_NO_WAYPOINTS);
        // user absolute path, i.e. leading /   
        if(pWPL_Store->Index == 0)
        {
                sprintf(filename, "/default.wpl");
        }

        }
        return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
}

 
 
// save actual gps positiin and heading to file
u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
{
        u8 retval = WPL_ERROR;

        u8 filename[30];
        Point_t WP;
 
        UART1_PutString("\n\r write single point\n\r");
        if(GPSData.Position.Status == INVALID)
         {

                UART1_PutString("ERROR: No GPS - Fix\n\r");
                return(retval);
         }
 

        WP.ToleranceRadius = 15;
        WP.HoldTime  = 5;
        WP.Index  = 1;
        WP.Type = POINT_TYPE_WP;
        WP.WP_EventChannelValue = 0;
        if(FC.StatusFlags & FC_STATUS_FLY)
         {
          WP.AltitudeRate = 15;
          WP.Position.Altitude = NaviData.Altimeter / 2;
         }
        else
        {
         WP.AltitudeRate = 0;
     WP.Position.Altitude = 0;
        }
        WP.Speed = SD_ComingHomeSpeed;
        WP.CamAngle = 0;
        WP.Name[0] = 'P';
        WP.Name[1] = 0;
        // add this point to wp list
        PointList_SetAt(&WP);

 
        sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
        UART1_PutString(filename);
        sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);

        retval = PointList_Save(filename, pWPL_Store->Name, 1);
 
        // clear current point list
        PointList_Clear();
 
        return(retval);
}
// load target gps posititon and heading from file
u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
{
        u8 filename[30];
       
        sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
        pWPL_Store->Name[0] = 0; // clear current list name

        return PointList_Load(filename, pWPL_Store->Name, sizeof(pWPL_Store->Name));
}
 
 
 
void ClearWLP_Name(void)

{
        u8 retval = 0;
        s32 altitude;
        GPS_Pos_t OldRefPos;
        GPS_Pos_Deviation_t RefDeviation;
         
        // check inputs for plausibility;
        if((RefIndex == 0) || (RefIndex > PointCount)) return(retval); 
        if(pRefPos == NULL) return(retval);
        if(pRefPos->Status == INVALID) return(retval);

 
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &OldRefPos)) // backup old reference position

                        altitude = PointList[i].Position.Altitude;
                        // calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position
                        if(!GPSPos_Deviation(&(PointList[i].Position), &OldRefPos, &RefDeviation)) break;
                        // copy of the new reference position into this list place
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
                        // restore former altitude 
                        PointList[i].Position.Altitude = altitude;
                        // move new reference according to the deviation of the old reference
                        if(RotationAngle > 0)
                        {
                                retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360, RefDeviation.Distance );

                        else // no rotation
                        {
                                // move new reference according to the deviation of the old reference
                                retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East);
                        }
                        if(!retval) break;             
                }
        } // else ref pos old not copied!
        if(!retval) PointList_Clear();
        return(retval);
}

 

Rev 515	Rev 516
Line 5...	Line 5...
5	// + www.MikroKopter.com	5	// + www.MikroKopter.com
6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7	// + Software Nutzungsbedingungen (english version: see below)	7	// + Software Nutzungsbedingungen (english version: see below)
8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -	8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den	9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool	10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.	11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.	12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im	14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.	15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie	16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.	17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren	18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren	20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand	21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22	// + des Mitverschuldens offen.	22	// + des Mitverschuldens offen.
23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.	23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.	25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang	26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
Line 30...	Line 30...
30	// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.	30	// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32	// + Software LICENSING TERMS	32	// + Software LICENSING TERMS
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -	34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35	// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware	35	// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.	36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37	// + The Software may only be used with the Licensor's products.	37	// + The Software may only be used with the Licensor's products.
38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this	38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this	39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40	// + agreement shall be the property of the Licensor.	40	// + agreement shall be the property of the Licensor.
Line 53...	Line 53...
53	// + #### END OF LICENSING TERMS ####	53	// + #### END OF LICENSING TERMS ####
54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.	54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56	#include <ctype.h>	56	#include <ctype.h>
57	#include <stdio.h>	57	#include <stdio.h>
58	#include <stdlib.h>	58	#include <stdlib.h>
59	#include <string.h>	59	#include <string.h>
60	#include "91x_lib.h"	60	#include "91x_lib.h"
61	#include "waypoints.h"	61	#include "waypoints.h"
62	#include "uart1.h"	62	#include "uart1.h"
63	#include "fat16.h"	63	#include "fat16.h"
Line 79...	Line 79...
79		79
Line 80...	Line 80...
80	u8 WPActive = FALSE;	80	u8 WPActive = FALSE;
81		81
82	u8 PointList_Init(void)	82	u8 PointList_Init(void)
83	{	83	{
Line 84...	Line 84...
84	return PointList_Clear();	84	return PointList_Clear();
85	}	85	}
86		86
87	u8 PointList_Clear(void)	87	u8 PointList_Clear(void)
88	{	88	{
89	u8 i;	89	u8 i;
90	WPIndex = 0; // real list position are 1 ,2, 3 ...	90	WPIndex = 0; // real list position are 1 ,2, 3 ...
91	POIIndex = 0; // real list position are 1 ,2, 3 ...	91	POIIndex = 0; // real list position are 1 ,2, 3 ...
92	WPCount = 0; // no waypoints	92	WPCount = 0; // no waypoints
93	POICount = 0;	93	POICount = 0;
94	PointCount = 0; // no contents	94	PointCount = 0; // no contents
Line 113...	Line 113...
113	PointList[i].Speed = 0;	113	PointList[i].Speed = 0;
114	PointList[i].CamAngle = 0;	114	PointList[i].CamAngle = 0;
115	PointList[i].Name[0] = 0;	115	PointList[i].Name[0] = 0;
116	}	116	}
117	ClearWLP_Name();	117	ClearWLP_Name();
118	return TRUE;	118	return TRUE;
119	}	119	}
Line 120...	Line 120...
120		120
121	u8 PointList_GetCount(void)	121	u8 PointList_GetCount(void)
122	{	122	{
Line 147...	Line 147...
147		147
148	case POINT_TYPE_WP:	148	case POINT_TYPE_WP:
149	WPCount++;	149	WPCount++;
150	PointCount++;	150	PointCount++;
151	break;	151	break;
152		152
153	case POINT_TYPE_POI:	153	case POINT_TYPE_POI:
154	POICount++;	154	POICount++;
155	PointCount++;	155	PointCount++;
156	break;	156	break;
157	}	157	}
158	break;	158	break;
159		159
160	case POINT_TYPE_WP: // was a waypoint	160	case POINT_TYPE_WP: // was a waypoint
161	switch(pPoint->Type)	161	switch(pPoint->Type)
162	{	162	{
163	case POINT_TYPE_INVALID:	163	case POINT_TYPE_INVALID:
Line 167...	Line 167...
167		167
168	default:	168	default:
169	case POINT_TYPE_WP:	169	case POINT_TYPE_WP:
170	//nothing to do	170	//nothing to do
171	break;	171	break;
172		172
173	case POINT_TYPE_POI:	173	case POINT_TYPE_POI:
174	POICount++;	174	POICount++;
175	WPCount--;	175	WPCount--;
176	break;	176	break;
177	}	177	}
178	break;	178	break;
179		179
180	case POINT_TYPE_POI: // was a poi	180	case POINT_TYPE_POI: // was a poi
181	switch(pPoint->Type)	181	switch(pPoint->Type)
182	{	182	{
183	case POINT_TYPE_INVALID:	183	case POINT_TYPE_INVALID:
Line 187...	Line 187...
187		187
188	case POINT_TYPE_WP:	188	case POINT_TYPE_WP:
189	WPCount++;	189	WPCount++;
190	POICount--;	190	POICount--;
191	break;	191	break;
192		192
193	case POINT_TYPE_POI:	193	case POINT_TYPE_POI:
194	default:	194	default:
195	// nothing to do	195	// nothing to do
196	break;	196	break;
197	}	197	}
198	break;	198	break;
199	}	199	}
200	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry	200	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
201	NaviData.WaypointNumber = WPCount;	201	NaviData.WaypointNumber = WPCount;
202	return pPoint->Index;	202	return pPoint->Index;
203	}	203	}
204	else return(0);	204	else return(0);
Line 211...	Line 211...
211	WPIndex = 0; // set list position invalid	211	WPIndex = 0; // set list position invalid
Line 212...	Line 212...
212		212
Line 213...	Line 213...
213	if(WPActive == FALSE) return(NULL);	213	if(WPActive == FALSE) return(NULL);
214		214
215	POIIndex = 0; // set invalid POI	215	POIIndex = 0; // set invalid POI
216	if(PointCount > 0)	216	if(PointCount > 0)
217	{	217	{
218	// search for first wp in list	218	// search for first wp in list
219	for(i = 0; i <MAX_LIST_LEN; i++)	219	for(i = 0; i <MAX_LIST_LEN; i++)
Line 227...	Line 227...
227	if(WPIndex) // found a WP in the list	227	if(WPIndex) // found a WP in the list
228	{	228	{
229	NaviData.WaypointIndex = 1;	229	NaviData.WaypointIndex = 1;
230	// update index to POI	230	// update index to POI
231	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	231	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
232	else POIIndex = 0;	232	else POIIndex = 0;
233	}	233	}
234	else // some points in the list but no WP found	234	else // some points in the list but no WP found
235	{	235	{
236	NaviData.WaypointIndex = 0;	236	NaviData.WaypointIndex = 0;
237	//Check for an existing POI	237	//Check for an existing POI
Line 245...	Line 245...
245	}	245	}
246	}	246	}
247	}	247	}
248	else // no point in the list	248	else // no point in the list
249	{	249	{
250	POIIndex = 0;	250	POIIndex = 0;
251	NaviData.WaypointIndex = 0;	251	NaviData.WaypointIndex = 0;
252	}	252	}
Line 253...	Line 253...
253		253
254	if(WPIndex) return(&(PointList[WPIndex-1]));	254	if(WPIndex) return(&(PointList[WPIndex-1]));
255	else return(NULL);	255	else return(NULL);
Line 256...	Line 256...
256	}	256	}
257		257
258	// returns the last waypoint	258	// returns the last waypoint
259	Point_t* PointList_WPEnd(void)	259	Point_t* PointList_WPEnd(void)
260	{	260	{
261		261
262	u8 i;	262	u8 i;
Line 263...	Line 263...
263	WPIndex = 0; // set list position invalid	263	WPIndex = 0; // set list position invalid
Line 269...	Line 269...
269	{	269	{
270	// search backward!	270	// search backward!
271	for(i = 1; i <= MAX_LIST_LEN; i++)	271	for(i = 1; i <= MAX_LIST_LEN; i++)
272	{	272	{
273	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	273	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
274	{	274	{
275	WPIndex = MAX_LIST_LEN - i + 1;	275	WPIndex = MAX_LIST_LEN - i + 1;
276	break;	276	break;
277	}	277	}
278	}	278	}
279	if(WPIndex) // found a WP within the list	279	if(WPIndex) // found a WP within the list
280	{	280	{
281	NaviData.WaypointIndex = WPCount;	281	NaviData.WaypointIndex = WPCount;
282	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	282	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
283	else POIIndex = 0;	283	else POIIndex = 0;
284	}	284	}
285	else // list contains some points but no WP in the list	285	else // list contains some points but no WP in the list
286	{	286	{
287	// search backward for a POI!	287	// search backward for a POI!
288	for(i = 1; i <= MAX_LIST_LEN; i++)	288	for(i = 1; i <= MAX_LIST_LEN; i++)
289	{	289	{
290	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	290	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
291	{	291	{
292	POIIndex = MAX_LIST_LEN - i + 1;	292	POIIndex = MAX_LIST_LEN - i + 1;
293	break;	293	break;
294	}	294	}
295	}	295	}
296	NaviData.WaypointIndex = 0;	296	NaviData.WaypointIndex = 0;
297	}	297	}
298	}	298	}
299	else // no point in the list	299	else // no point in the list
300	{	300	{
301	POIIndex = 0;	301	POIIndex = 0;
Line 308...	Line 308...
308	// returns a pointer to the next waypoint or NULL if the end of the list has been reached	308	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
309	Point_t* PointList_WPNext(void)	309	Point_t* PointList_WPNext(void)
310	{	310	{
311	u8 wp_found = 0;	311	u8 wp_found = 0;
312	if(WPActive == FALSE) return(NULL);	312	if(WPActive == FALSE) return(NULL);
313		313
314	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list	314	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
315	{	315	{
316	u8 i;	316	u8 i;
317	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry	317	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
318	{	318	{
Line 329...	Line 329...
329	NaviData.WaypointIndex++;	329	NaviData.WaypointIndex++;
330	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	330	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
331	else POIIndex = 0;	331	else POIIndex = 0;
332	return(&(PointList[WPIndex-1])); // return pointer to this waypoint	332	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
333	}	333	}
334	else	334	else
335	{ // no next wp found	335	{ // no next wp found
336	NaviData.WaypointIndex = 0;	336	NaviData.WaypointIndex = 0;
337	POIIndex = 0;	337	POIIndex = 0;
338	return(NULL);	338	return(NULL);
339	}	339	}
340	}	340	}
Line 341...	Line 341...
341		341
342	void PointList_WPActive(u8 set)	342	void PointList_WPActive(u8 set)
343	{	343	{
344	if(set)	344	if(set)
345	{	345	{
346	WPActive = TRUE;	346	WPActive = TRUE;
347	PointList_WPBegin(); // updates POI index	347	PointList_WPBegin(); // updates POI index
348	}	348	}
349	else	349	else
350	{	350	{
351	WPActive = FALSE;	351	WPActive = FALSE;
352	POIIndex = 0; // disable POI also	352	POIIndex = 0; // disable POI also
353	}	353	}
354	}	354	}
355		355
356	Point_t* PointList_GetPOI(void)	356	Point_t* PointList_GetPOI(void)
357	{	357	{
358	return PointList_GetAt(POIIndex);	358	return PointList_GetAt(POIIndex);
Line 359...	Line 359...
359	}	359	}
360		360
Line 372...	Line 372...
372		372
373	if(Fat16_IsValid())	373	if(Fat16_IsValid())
374	{ // check if wpl file is existing	374	{ // check if wpl file is existing
375	if(fexist_(filename))	375	if(fexist_(filename))
376	{ //file is existent	376	{ //file is existent
377	if(!(overwride))	377	if(!(overwride))
378	{	378	{
379	UART1_PutString("Error: file exist!\r\n");	379	UART1_PutString("Error: file exist!\r\n");
380	return(WPL_FILEEXIST);	380	return(WPL_FILEEXIST);
381	}	381	}
382	}	382	}
383	fp = fopen_(filename, 'w'); // try to open the file	383	fp = fopen_(filename, 'w'); // try to open the file
384	if(fp == NULL)	384	if(fp == NULL)
385	{	385	{
386	UART1_PutString("ERROR: Creating waypoint file!\r\n");	386	UART1_PutString("ERROR: Creating waypoint file!\r\n");
387	return(retval);	387	return(retval);
388	}	388	}
389	// Create general section and key entries	389	// Create general section and key entries
390	fputs_("[General]\r\n", fp);	390	fputs_("[General]\r\n", fp);
391	sprintf(wpline, "Name=%s\r\n", listname);	391	sprintf(wpline, "Name=%s\r\n", listname);
392	fputs_(wpline, fp);	392	fputs_(wpline, fp);
393	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);	393	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
394	fputs_(wpline, fp);	394	fputs_(wpline, fp);
395	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);	395	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
396	fputs_(wpline, fp);	396	fputs_(wpline, fp);
397	// dump all points if existent	397	// dump all points if existent
Line 454...	Line 454...
454	// write cam angle in degree (255 -> POI-Automatic)	454	// write cam angle in degree (255 -> POI-Automatic)
455	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);	455	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
456	fputs_(wpline, fp);	456	fputs_(wpline, fp);
457	// write point type	457	// write point type
458	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);	458	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
459	fputs_(wpline, fp);	459	fputs_(wpline, fp);
460	// write prefix	460	// write prefix
461	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);	461	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
462	fputs_(wpline, fp);	462	fputs_(wpline, fp);
463	} // EOF loop over all points	463	} // EOF loop over all points
464	} // EOF if(PointCount)	464	} // EOF if(PointCount)
465	if(EOF == fclose_(fp))	465	if(EOF == fclose_(fp))
Line 468...	Line 468...
468	}	468	}
469	else	469	else
470	{	470	{
471	UART1_PutString("ok\r\n");	471	UART1_PutString("ok\r\n");
472	retval = WPL_OK;	472	retval = WPL_OK;
473	}	473	}
474	} // EOF if(Fat16_IsValid())	474	} // EOF if(Fat16_IsValid())
475	else	475	else
476	{	476	{
477	UART1_PutString("no file system found!\r\n");	477	UART1_PutString("no file system found!\r\n");
478	retval = WPL_NO_SDCARD_FOUND;	478	retval = WPL_NO_SDCARD_FOUND;
479	}	479	}
480	return(retval);	480	return(retval);
Line 483...	Line 483...
483	u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen)	483	u8 PointList_Load(u8 * filename, u8* listname, u8 listnamelen)
484	{	484	{
485	File_t *fp;	485	File_t *fp;
486	s8 wpline[LINE_MAX];	486	s8 wpline[LINE_MAX];
487	u8 retval = WPL_ERROR;	487	u8 retval = WPL_ERROR;
488		488
489	s8 name, value;	489	s8 name, value;
490	u8 i;	490	u8 i;
Line 491...	Line 491...
491		491
492	u8 IsGeneralSection = 0;	492	u8 IsGeneralSection = 0;
Line 495...	Line 495...
495		495
496	// clear point list first	496	// clear point list first
Line 497...	Line 497...
497	PointList_Clear();	497	PointList_Clear();
498		498
499	UART1_PutString("\n\r Read ");	499	UART1_PutString("\n\r Read ");
Line 500...	Line 500...
500	UART1_PutString(filename);	500	UART1_PutString(filename);
501	UART1_PutString("...");	501	UART1_PutString("...");
502		502
Line 602...	Line 602...
602	else if(strcmp(name, "TYPE") == 0)	602	else if(strcmp(name, "TYPE") == 0)
603	{	603	{
604	PointList[IsPointSection-1].Type = (u8)atoi(value);	604	PointList[IsPointSection-1].Type = (u8)atoi(value);
605	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift	605	if(PointList[IsPointSection-1].Type > 0) PointList[IsPointSection-1].Type--; // index shift
606	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;	606	else PointList[IsPointSection-1].Type = POINT_TYPE_INVALID;
607		607
608	switch(PointList[IsPointSection-1].Type)	608	switch(PointList[IsPointSection-1].Type)
609	{	609	{
610	case POINT_TYPE_WP:	610	case POINT_TYPE_WP:
611	// this works only if altitude key is set before point type key in WPL file !!	611	// this works only if altitude key is set before point type key in WPL file !!
612	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs	612	PointList[IsPointSection-1].Position.Altitude /= 10; // dm only for WPs
613	WPCount++;	613	WPCount++;
614	break;	614	break;
Line 615...	Line 615...
615		615
616	case POINT_TYPE_POI:	616	case POINT_TYPE_POI:
Line 626...	Line 626...
626	else	626	else
627	{	627	{
628	UART1_PutString("Unknown key: ");	628	UART1_PutString("Unknown key: ");
629	UART1_PutString(name);	629	UART1_PutString(name);
630	UART1_PutString("\r\n");	630	UART1_PutString("\r\n");
631	}	631	}
632	} // EOF point section	632	} // EOF point section
633	else if(IsGeneralSection)	633	else if(IsGeneralSection)
634	{	634	{
635	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)	635	if(strcmp(name, "NUMBEROFWAYPOINTS") == 0)
636	{	636	{
637	WPNumber = (u8)atoi(value);	637	WPNumber = (u8)atoi(value);
638	if(!WPNumber) // no waypoints in file	638	if(!WPNumber) // no waypoints in file
639	{	639	{
640	return(WPL_NO_WAYPOINTS); // we are done here	640	return(WPL_NO_WAYPOINTS); // we are done here
641	}	641	}
642	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list	642	else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
643	{	643	{
644	UART1_PutString("To many points!");	644	UART1_PutString("To many points!");
645	return(WPL_ERROR);	645	return(WPL_ERROR);
646	}	646	}
647	}	647	}
648	else if (strcmp(name, "FILEVERSION") == 0)	648	else if (strcmp(name, "FILEVERSION") == 0)
649	{	649	{
650	if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)	650	if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
651	{	651	{
652	PointList_Clear();	652	PointList_Clear();
653	UART1_PutString("Bad file version!\r\n");	653	UART1_PutString("Bad file version!\r\n");
654	return(WPL_ERROR);	654	return(WPL_ERROR);
655	}	655	}
656	}	656	}
657	else if (strcmp(name, "NAME") == 0)	657	else if (strcmp(name, "NAME") == 0)
658	{	658	{
659	if(listname)	659	if(listname)
Line 687...	Line 687...
687	} // EOF key entry line	687	} // EOF key entry line
688	} // valid line	688	} // valid line
689	} // EOF loop over all lines	689	} // EOF loop over all lines
690	fclose_(fp);	690	fclose_(fp);
691	NaviData.WaypointNumber = WPCount;	691	NaviData.WaypointNumber = WPCount;
692	retval = WPL_OK;	692	retval = WPL_OK;
693	UART1_PutString("ok\r\n");	693	UART1_PutString("ok\r\n");
694	} // EOF if(Fat16_IsValid())	694	} // EOF if(Fat16_IsValid())
695	else	695	else
696	{	696	{
697	UART1_PutString("no file system found!\r\n");	697	UART1_PutString("no file system found!\r\n");
698	retval = WPL_NO_SDCARD_FOUND;	698	retval = WPL_NO_SDCARD_FOUND;
699	}	699	}
700	return(retval);	700	return(retval);
701	}	701	}
Line 702...	Line 702...
702		702
703	// load actual point list from SD card	703	// load actual point list from SD card
704	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)	704	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
705	{	705	{
Line 706...	Line 706...
706	u8 filename[30];	706	u8 filename[30];
Line 707...	Line 707...
707		707
708	pWPL_Store->Name[0] = 0; // clear current list name	708	pWPL_Store->Name[0] = 0; // clear current list name
709		709
710	// user absolute path, i.e. leading /	710	// user absolute path, i.e. leading /
711	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root	711	if(pWPL_Store->Index == 0) // index 0 looks for a default WPL file in the root
712	{	712	{
713	sprintf(filename, "/default.wpl");	713	sprintf(filename, "/default.wpl");
714	}	714	}
715	else	715	else
Line 721...	Line 721...
721		721
722	// save actual point list to SD card	722	// save actual point list to SD card
723	u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)	723	u8 PointList_WriteToFile(WPL_Store_t * pWPL_Store)
724	{	724	{
725	u8 filename[30];	725	u8 filename[30];
726		726
727		727
728	if(PointCount == 0) return(WPL_NO_WAYPOINTS);	728	if(PointCount == 0) return(WPL_NO_WAYPOINTS);
729	// user absolute path, i.e. leading /	729	// user absolute path, i.e. leading /
730	if(pWPL_Store->Index == 0)	730	if(pWPL_Store->Index == 0)
731	{	731	{
732	sprintf(filename, "/default.wpl");	732	sprintf(filename, "/default.wpl");
733	}	733	}
Line 737...	Line 737...
737	}	737	}
738	return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);	738	return PointList_Save(filename, pWPL_Store->Name, pWPL_Store->OverwriteFile);
739	}	739	}
Line 740...	Line 740...
740		740
741		741
742	// save actual gps position and heading to file	742	// save actual gps positiin and heading to file
743	u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)	743	u8 PointList_SaveSinglePoint(WPL_Store_t * pWPL_Store)
744	{	744	{
745	u8 retval = WPL_ERROR;	745	u8 retval = WPL_ERROR;
Line 746...	Line 746...
746	u8 filename[30];	746	u8 filename[30];
747	Point_t WP;	747	Point_t WP;
748		748
749	UART1_PutString("\n\r write single point\n\r");	749	UART1_PutString("\n\r write single point\n\r");
750	if(GPSData.Position.Status == INVALID)	750	if(GPSData.Position.Status == INVALID)
751	{	751	{
Line 752...	Line 752...
752	UART1_PutString("ERROR: No GPS - Fix\n\r");	752	UART1_PutString("ERROR: No GPS - Fix\n\r");
753	return(retval);	753	return(retval);
754	}	754	}
755		755
Line 763...	Line 763...
763	WP.ToleranceRadius = 15;	763	WP.ToleranceRadius = 15;
764	WP.HoldTime = 5;	764	WP.HoldTime = 5;
765	WP.Index = 1;	765	WP.Index = 1;
766	WP.Type = POINT_TYPE_WP;	766	WP.Type = POINT_TYPE_WP;
767	WP.WP_EventChannelValue = 0;	767	WP.WP_EventChannelValue = 0;
768	if(FC.StatusFlags & FC_STATUS_FLY && (NaviData.Altimeter) > 8 * 20)	768	if(FC.StatusFlags & FC_STATUS_FLY)
769	{	769	{
770	WP.AltitudeRate = 30;	770	WP.AltitudeRate = 15;
771	WP.Position.Altitude = NaviData.Altimeter / 2;	771	WP.Position.Altitude = NaviData.Altimeter / 2;
772	}	772	}
773	else	773	else
774	{	774	{
775	WP.AltitudeRate = 0;	775	WP.AltitudeRate = 0;
776	WP.Position.Altitude = 0;	776	WP.Position.Altitude = 0;
777	}	777	}
778	WP.Speed = SD_ComingHomeSpeed;	778	WP.Speed = SD_ComingHomeSpeed;
779	WP.CamAngle = 0;	779	WP.CamAngle = 0;
780	WP.Name[0] = 'P';	780	WP.Name[0] = 'P';
781	WP.Name[1] = 0;	781	WP.Name[1] = 0;
782	// add this point to wp list	782	// add this point to wp list
783	PointList_SetAt(&WP);	783	PointList_SetAt(&WP);
Line 784...	Line 784...
784		784
785	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);	785	sprintf(filename, "/POINT/POINT%03d.wpl", pWPL_Store->Index);
786	UART1_PutString(filename);	786	UART1_PutString(filename);
787	sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);	787	sprintf(pWPL_Store->Name, "POINT%03d ", pWPL_Store->Index);
Line 788...	Line 788...
788	retval = PointList_Save(filename, pWPL_Store->Name, 1);	788	retval = PointList_Save(filename, pWPL_Store->Name, 1);
789		789
790	// clear current point list	790	// clear current point list
791	PointList_Clear();	791	PointList_Clear();
792	return(retval);	-
793	}	792	return(retval);
794		793	}
795	// load target gps posititon and heading from file	794	// load target gps posititon and heading from file
796	u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)	795	u8 PointList_LoadSinglePoint(WPL_Store_t * pWPL_Store)
797	{	796	{
798	u8 filename[30];	797	u8 filename[30];
799		798

Subversion Repositories NaviCtrl

(root)/tags/V2.10e/waypoints.c @ 770 - Rev 515 → 516