WebSVN - NaviCtrl - Diff - Rev 497 and 498


/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + 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
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// +  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.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### 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"
 
 
WPL_Store_t WPL_Store;
 
// the waypoints list
#define MAX_LIST_LEN 101
 
Point_t PointList[MAX_LIST_LEN];
u8 WPIndex = 0;         // list index of GPS point representig the current WP, can be maximal WPCount
u8 POIIndex = 0;        // list index of GPS Point representing the current POI, can be maximal WPCount
u8 WPCount = 0;         // number of waypoints
u8 PointCount = 0;      // number of points in the list can be maximal equal to MAX_LIST_LEN
u8 POICount = 0;        // number of point of interest in the list
 
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
        WPActive = FALSE;
        NaviData.WaypointNumber = WPCount;
        NaviData.WaypointIndex = 0;
 
        for(i = 0; i < MAX_LIST_LEN; i++)
        {
                PointList[i].Position.Status = INVALID;
                PointList[i].Position.Latitude = 0;
                PointList[i].Position.Longitude = 0;
                PointList[i].Position.Altitude = 0;
                PointList[i].Heading = 361;             // invalid value
                PointList[i].ToleranceRadius = 0;       // in meters, if the MK is within that range around the target, then the next target is triggered
                PointList[i].HoldTime = 0;                      // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
                PointList[i].Event_Flag = 0;            // future implementation
                PointList[i].Index = 0;
                PointList[i].Type = POINT_TYPE_INVALID;
                PointList[i].WP_EventChannelValue = 0;
                PointList[i].AltitudeRate = 0;          // no change of setpoint
                PointList[i].Speed = 0;
                PointList[i].CamAngle = 0;
                PointList[i].Name[0] = 0;
        }
        return TRUE;           
}
 
u8 PointList_GetCount(void)
{
        return PointCount; // number of points in the list
}
 
Point_t* PointList_GetAt(u8 index)
{
        if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
        else return(NULL);
}
 
u8 PointList_SetAt(Point_t* pPoint)
{
        // if index is in range
        if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
        {
                // check list entry before update
                switch(PointList[pPoint->Index-1].Type)
                {
                        case POINT_TYPE_INVALID: // was invalid
                                switch(pPoint->Type)
                                {
                                        default:
                                        case POINT_TYPE_INVALID:
                                                // nothing to do
                                                break;
 
                                        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:
                                                WPCount--;
                                                PointCount--;
                                                break;
 
                                        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:
                                                POICount--;
                                                PointCount--;
                                                break;
 
                                        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);
}
 
// returns the pointer to the first waypoint within the list
Point_t* PointList_WPBegin(void)
{
        u8 i;
        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((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
                        {
                                WPIndex = i + 1;
                                break;
                        }
                }
                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
                        for(i = 0; i < MAX_LIST_LEN; i++)
                        {
                                if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
                                {
                                        POIIndex = i + 1;
                                        break;
                                }
                        }
                }
        }
        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
        POIIndex = 0; // set invalid
 
        if(WPActive == FALSE) return(NULL);
 
        if(PointCount > 0)
        {
                // 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;
                NaviData.WaypointIndex = 0;
        }
        if(WPIndex) return(&(PointList[WPIndex-1]));
        else return(NULL);
}
 
// 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
                {
                        if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
                        {
                                wp_found = i+1;
                                break;
                        }
                }
        }
        if(wp_found)
        {
                WPIndex = wp_found; // update list position
                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);      
}
 
#define LINE_MAX 70
#define WP_FILE_VERSION_COMPATIBLE 3
// save actual point list to SD card
u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store)
{
        File_t *fp;
        s8 wpline[LINE_MAX];
        u8 retval = WPL_ERROR;
       
        if(PointCount == 0) return(WPL_NO_WAYPOINTS);
        // user absolute path, i.e. leading /   
        if(pWPL_Store->Index == 0)
        {
                sprintf(wpline, "/default.wpl");
        }
        else
        {
                sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);
        }
        UART1_PutString("\n\r Save WPL...");
 
        if(Fat16_IsValid())
        {       // check if wpl file is existing
                if(fexist_(wpline))
                {       //file is existent
                        if(!(pWPL_Store->OverwriteFile))
                        {
                                UART1_PutString("Error: file exist!\r\n");
                                return(WPL_FILEEXIST);
                        }      
                }
                fp = fopen_(wpline, '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",  pWPL_Store->Name);
                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
                if(PointCount)
                {
                        u8 i, u8_1;
                        s32 i32_1, i32_2;
 
                        for (i = 0; i < PointCount; i++)
                        {
                                sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
                                fputs_(wpline, fp);
                                // write latitude in deg
                                if(PointList[i].Position.Latitude < 0) u8_1 = '-';
                                else u8_1 = '+';
                                i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
                                i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
                                sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
                                fputs_(wpline, fp);
                                // write longitude in deg
                                if(PointList[i].Position.Longitude < 0) u8_1 = '-';
                                else u8_1 = '+';
                                i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
                                i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
                                sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
                                fputs_(wpline, fp);
                                // write tolerace radius in m
                                sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
                                fputs_(wpline, fp);
                                // write altitude in m
                                if(PointList[i].Position.Altitude < 0) u8_1 = '-';
                                else u8_1 = '+';
                                if(PointList[i].Type == POINT_TYPE_POI)
                                {
                                        i32_1 = abs(PointList[i].Position.Altitude)/100L;  // cm --> m
                                        i32_2 = abs(PointList[i].Position.Altitude)%100L;
                                }
                                else
                                {
                                        i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
                                        i32_2 = abs(PointList[i].Position.Altitude)%10L;
                                }
                                sprintf(wpline, "Altitude=%c%ld.%01ld\r\n",  u8_1, i32_1, i32_2);
                                fputs_(wpline, fp);
                                // write climb rate in 0.1 m/s
                                sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
                                fputs_(wpline, fp);
                                // write hold time in s
                                sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
                                fputs_(wpline, fp);
                                // write event channel value
                                sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
                                fputs_(wpline, fp);
                                // write heading in deg (0= nothing, neg. values index to poi)
                                sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
                                fputs_(wpline, fp);
                                // write speed in 0.1 m/s
                                sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
                                fputs_(wpline, fp);
                                // 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))
                {
                        UART1_PutString("failed!\r\n");
                }
                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);
}
 
// load actual point list from SD card
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
{
        File_t *fp;
        s8 wpline[LINE_MAX];
        u8 retval = WPL_ERROR;
       
        s8 *name, *value;
        u8 i;
 
        u8 IsGeneralSection = 0;
        u8 IsPointSection  = 0;
        u8 WPNumber = 0;
 
 
        // clear point list first
        PointList_Clear();
        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(wpline, "/default.wpl");       
        }
        else
        {
                sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);
        }
 
        UART1_PutString("\n\r Read ");
        UART1_PutString(wpline);
        UART1_PutString("...");
 
        if(Fat16_IsValid())
        {       // check if wpl file is existing
                fp = fopen_(wpline, 'r');               // try to open the file
                if(fp == NULL)
                {
                        UART1_PutString("ERROR: Reading waypoint file!\r\n");
                        return(retval);
                }
 
 
                // read all lines from file
                while(fgets_(wpline, LINE_MAX, fp) != 0)
                {
                        if ( // ignorelines starting with \r,\n,' ',';','#'
                                (wpline[0] != '\n') &&
                                (wpline[0] != '\r') &&
                                (wpline[0] != ' ' ) &&
                                (wpline[0] != ';' ) &&
                                (wpline[0] != '#' )
                                )
                        {
                                // check for section line found
                                if(wpline[0] == '[')
                                {
                                        // next section found
                                        IsGeneralSection = 0;
                                        IsPointSection = 0;
 
                                        name  = strtok(&wpline[1], "]");
                                        if(name != NULL)   // if section name
                                        {
                                                // check section type
                                                for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
 
                                                if(strncmp(name, "POINT", 5) == 0)
                                                {
                                                        IsPointSection = (u8)atoi(&name[5]);
                                                        PointCount++;
                                                }
                                                else if(strcmp(name, "GENERAL") == 0)
                                                {
                                                        IsGeneralSection = 1;
                                                }
                                                else
                                                {
                                                        UART1_PutString("Unknown section: ");
                                                        UART1_PutString(name);
                                                        UART1_PutString("\r\n");
                                                }
                                        }
                                } // EOF section line
                                else
                                {       // look for key entrys of each sections
                                        name  = strtok(wpline, "="); // get name
                                        value = strtok(NULL, "="); // get value
                                        if ((name != NULL) && (value != NULL))
                                        {
                                                for(i=0; name[i]; i++) name[i] = toupper(name[i]); // convert to upper case
                                                if(IsPointSection &&  (IsPointSection <= WPNumber))
                                                {
                                                        if(strcmp(name, "LATITUDE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Position.Latitude = (s32)(atof(value) * 1E7);
                                                        }
                                                        else if(strcmp(name, "LONGITUDE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Position.Longitude = (s32)(atof(value) * 1E7);
                                                        }
                                                        else if(strcmp(name, "RADIUS") == 0)
                                                        {
                                                                PointList[IsPointSection-1].ToleranceRadius = (u8)atoi(value);
                                                        }
                                                        else if(strcmp(name, "ALTITUDE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Position.Altitude = (s32)(atof(value) * 100.0);      // in cm
                                                                PointList[IsPointSection-1].Position.Status = NEWDATA;
                                                        }
                                                        else if(strcmp(name, "CLIMBRATE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].AltitudeRate = (u8)atoi(value);
                                                        }
                                                        else if(strcmp(name, "DELAYTIME") == 0)
                                                        {
                                                                PointList[IsPointSection-1].HoldTime = (u8)atoi(value);
                                                        }
                                                        else if(strcmp(name, "WP_EVENT_CHANNEL_VALUE") == 0)
                                                        {
                                                                PointList[IsPointSection-1].WP_EventChannelValue = (u8)atoi(value);
                                                        }
                                                        else if(strcmp(name, "HEADING") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Heading = (s16)atoi(value);
                                                        }
                                                        else if(strcmp(name, "SPEED") == 0)
                                                        {
                                                                PointList[IsPointSection-1].Speed = (u8)atoi(value);
                                                        }
                                                        else if(strcmp(name, "CAM-NICK") == 0)
                                                        {
                                                                PointList[IsPointSection-1].CamAngle = (u8)atoi(value);
                                                        }
                                                        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:
                                                                                POICount++;
                                                                                break;
                                                                }
                                                        }
                                                        else if(strcmp(name, "PREFIX") == 0)
                                                        {
                                                                strncpy(PointList[IsPointSection-1].Name, value, 4);
                                                                PointList[IsPointSection-1].Name[3] = 0; // Terminate string
                                                        }
                                                        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 much 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)
                                                        {
                                                                u8 len = strlen(value);
                                                                if(value[len-1] == '\r')
                                                                {
                                                                        value[len-1] = 0;
                                                                        len--;
                                                                }
                                                                if(len > 11) value[11] = 0;
                                                                memcpy(pWPL_Store->Name, value, 12);
                                                        }
                                                        else
                                                        {
                                                                UART1_PutString("Unknown key: ");
                                                                UART1_PutString(name);
                                                                UART1_PutString("\r\n");
                                                        }
                                                } // EOF general section
                                        } // EOF valid key entry
                                } // 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);
}
 
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos)
{
        u8 retval = 0;
        GPS_Pos_t RefPos_old;
        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);
 
        // try to copy the old reference in point list to a local buffer
        if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &RefPos_old))
        {
                u8 i;
                // for each point position in the list
                for(i = 0; i < PointCount; i++)
                {
                        retval = 0;
                    // 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), &RefPos_old, &RefDeviation)) break;
                        // copy of the new reference position into this list place
                        if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break;
                        // 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 497	Rev 498
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.	27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.	28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'	29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
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.
41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other	41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42	// + features that can be used to identify the program may not be altered or defaced by the customer.	42	// + features that can be used to identify the program may not be altered or defaced by the customer.
43	// + The customer shall be responsible for taking reasonable precautions	43	// + The customer shall be responsible for taking reasonable precautions
44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the	44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and	45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product	46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.	47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test	48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.	49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data	50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.	51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.	52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
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"
64		64
65		65
66	WPL_Store_t WPL_Store;	66	WPL_Store_t WPL_Store;
67		67
68	// the waypoints list	68	// the waypoints list
69	#define MAX_LIST_LEN 101	69	#define MAX_LIST_LEN 101
70		70
71	Point_t PointList[MAX_LIST_LEN];	71	Point_t PointList[MAX_LIST_LEN];
72	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount	72	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
73	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount	73	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
74	u8 WPCount = 0; // number of waypoints	74	u8 WPCount = 0; // number of waypoints
75	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN	75	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN
76	u8 POICount = 0; // number of point of interest in the list	76	u8 POICount = 0; // number of point of interest in the list
77		77
78	u8 WPActive = FALSE;	78	u8 WPActive = FALSE;
79		79
80	u8 PointList_Init(void)	80	u8 PointList_Init(void)
81	{	81	{
82	return PointList_Clear();	82	return PointList_Clear();
83	}	83	}
84		84
85	u8 PointList_Clear(void)	85	u8 PointList_Clear(void)
86	{	86	{
87	u8 i;	87	u8 i;
88	WPIndex = 0; // real list position are 1 ,2, 3 ...	88	WPIndex = 0; // real list position are 1 ,2, 3 ...
89	POIIndex = 0; // real list position are 1 ,2, 3 ...	89	POIIndex = 0; // real list position are 1 ,2, 3 ...
90	WPCount = 0; // no waypoints	90	WPCount = 0; // no waypoints
91	POICount = 0;	91	POICount = 0;
92	PointCount = 0; // no contents	92	PointCount = 0; // no contents
93	WPActive = FALSE;	93	WPActive = FALSE;
94	NaviData.WaypointNumber = WPCount;	94	NaviData.WaypointNumber = WPCount;
95	NaviData.WaypointIndex = 0;	95	NaviData.WaypointIndex = 0;
96		96
97	for(i = 0; i < MAX_LIST_LEN; i++)	97	for(i = 0; i < MAX_LIST_LEN; i++)
98	{	98	{
99	PointList[i].Position.Status = INVALID;	99	PointList[i].Position.Status = INVALID;
100	PointList[i].Position.Latitude = 0;	100	PointList[i].Position.Latitude = 0;
101	PointList[i].Position.Longitude = 0;	101	PointList[i].Position.Longitude = 0;
102	PointList[i].Position.Altitude = 0;	102	PointList[i].Position.Altitude = 0;
103	PointList[i].Heading = 361; // invalid value	103	PointList[i].Heading = 361; // invalid value
104	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered	104	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
105	PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered	105	PointList[i].HoldTime = 0; // in seconds, if the was once in the tolerance area around a WP, this time defines the delay before the next WP is triggered
106	PointList[i].Event_Flag = 0; // future implementation	106	PointList[i].Event_Flag = 0; // future implementation
107	PointList[i].Index = 0;	107	PointList[i].Index = 0;
108	PointList[i].Type = POINT_TYPE_INVALID;	108	PointList[i].Type = POINT_TYPE_INVALID;
109	PointList[i].WP_EventChannelValue = 0;	109	PointList[i].WP_EventChannelValue = 0;
110	PointList[i].AltitudeRate = 0; // no change of setpoint	110	PointList[i].AltitudeRate = 0; // no change of setpoint
111	PointList[i].Speed = 0;	111	PointList[i].Speed = 0;
112	PointList[i].CamAngle = 0;	112	PointList[i].CamAngle = 0;
113	PointList[i].Name[0] = 0;	113	PointList[i].Name[0] = 0;
114	}	114	}
115	return TRUE;	115	return TRUE;
116	}	116	}
117		117
118	u8 PointList_GetCount(void)	118	u8 PointList_GetCount(void)
119	{	119	{
120	return PointCount; // number of points in the list	120	return PointCount; // number of points in the list
121	}	121	}
122		122
123	Point_t* PointList_GetAt(u8 index)	123	Point_t* PointList_GetAt(u8 index)
124	{	124	{
125	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint	125	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
126	else return(NULL);	126	else return(NULL);
127	}	127	}
128		128
129	u8 PointList_SetAt(Point_t* pPoint)	129	u8 PointList_SetAt(Point_t* pPoint)
130	{	130	{
131	// if index is in range	131	// if index is in range
132	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))	132	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
133	{	133	{
134	// check list entry before update	134	// check list entry before update
135	switch(PointList[pPoint->Index-1].Type)	135	switch(PointList[pPoint->Index-1].Type)
136	{	136	{
137	case POINT_TYPE_INVALID: // was invalid	137	case POINT_TYPE_INVALID: // was invalid
138	switch(pPoint->Type)	138	switch(pPoint->Type)
139	{	139	{
140	default:	140	default:
141	case POINT_TYPE_INVALID:	141	case POINT_TYPE_INVALID:
142	// nothing to do	142	// nothing to do
143	break;	143	break;
144		144
145	case POINT_TYPE_WP:	145	case POINT_TYPE_WP:
146	WPCount++;	146	WPCount++;
147	PointCount++;	147	PointCount++;
148	break;	148	break;
149		149
150	case POINT_TYPE_POI:	150	case POINT_TYPE_POI:
151	POICount++;	151	POICount++;
152	PointCount++;	152	PointCount++;
153	break;	153	break;
154	}	154	}
155	break;	155	break;
156		156
157	case POINT_TYPE_WP: // was a waypoint	157	case POINT_TYPE_WP: // was a waypoint
158	switch(pPoint->Type)	158	switch(pPoint->Type)
159	{	159	{
160	case POINT_TYPE_INVALID:	160	case POINT_TYPE_INVALID:
161	WPCount--;	161	WPCount--;
162	PointCount--;	162	PointCount--;
163	break;	163	break;
164		164
165	default:	165	default:
166	case POINT_TYPE_WP:	166	case POINT_TYPE_WP:
167	//nothing to do	167	//nothing to do
168	break;	168	break;
169		169
170	case POINT_TYPE_POI:	170	case POINT_TYPE_POI:
171	POICount++;	171	POICount++;
172	WPCount--;	172	WPCount--;
173	break;	173	break;
174	}	174	}
175	break;	175	break;
176		176
177	case POINT_TYPE_POI: // was a poi	177	case POINT_TYPE_POI: // was a poi
178	switch(pPoint->Type)	178	switch(pPoint->Type)
179	{	179	{
180	case POINT_TYPE_INVALID:	180	case POINT_TYPE_INVALID:
181	POICount--;	181	POICount--;
182	PointCount--;	182	PointCount--;
183	break;	183	break;
184		184
185	case POINT_TYPE_WP:	185	case POINT_TYPE_WP:
186	WPCount++;	186	WPCount++;
187	POICount--;	187	POICount--;
188	break;	188	break;
189		189
190	case POINT_TYPE_POI:	190	case POINT_TYPE_POI:
191	default:	191	default:
192	// nothing to do	192	// nothing to do
193	break;	193	break;
194	}	194	}
195	break;	195	break;
196	}	196	}
197	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry	197	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
198	NaviData.WaypointNumber = WPCount;	198	NaviData.WaypointNumber = WPCount;
199	return pPoint->Index;	199	return pPoint->Index;
200	}	200	}
201	else return(0);	201	else return(0);
202	}	202	}
203		203
204	// returns the pointer to the first waypoint within the list	204	// returns the pointer to the first waypoint within the list
205	Point_t* PointList_WPBegin(void)	205	Point_t* PointList_WPBegin(void)
206	{	206	{
207	u8 i;	207	u8 i;
208	WPIndex = 0; // set list position invalid	208	WPIndex = 0; // set list position invalid
209		209
210	if(WPActive == FALSE) return(NULL);	210	if(WPActive == FALSE) return(NULL);
211		211
212	POIIndex = 0; // set invalid POI	212	POIIndex = 0; // set invalid POI
213	if(PointCount > 0)	213	if(PointCount > 0)
214	{	214	{
215	// search for first wp in list	215	// search for first wp in list
216	for(i = 0; i <MAX_LIST_LEN; i++)	216	for(i = 0; i <MAX_LIST_LEN; i++)
217	{	217	{
218	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))	218	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
219	{	219	{
220	WPIndex = i + 1;	220	WPIndex = i + 1;
221	break;	221	break;
222	}	222	}
223	}	223	}
224	if(WPIndex) // found a WP in the list	224	if(WPIndex) // found a WP in the list
225	{	225	{
226	NaviData.WaypointIndex = 1;	226	NaviData.WaypointIndex = 1;
227	// update index to POI	227	// update index to POI
228	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	228	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
229	else POIIndex = 0;	229	else POIIndex = 0;
230	}	230	}
231	else // some points in the list but no WP found	231	else // some points in the list but no WP found
232	{	232	{
233	NaviData.WaypointIndex = 0;	233	NaviData.WaypointIndex = 0;
234	//Check for an existing POI	234	//Check for an existing POI
235	for(i = 0; i < MAX_LIST_LEN; i++)	235	for(i = 0; i < MAX_LIST_LEN; i++)
236	{	236	{
237	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))	237	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
238	{	238	{
239	POIIndex = i + 1;	239	POIIndex = i + 1;
240	break;	240	break;
241	}	241	}
242	}	242	}
243	}	243	}
244	}	244	}
245	else // no point in the list	245	else // no point in the list
246	{	246	{
247	POIIndex = 0;	247	POIIndex = 0;
248	NaviData.WaypointIndex = 0;	248	NaviData.WaypointIndex = 0;
249	}	249	}
250		250
251	if(WPIndex) return(&(PointList[WPIndex-1]));	251	if(WPIndex) return(&(PointList[WPIndex-1]));
252	else return(NULL);	252	else return(NULL);
253	}	253	}
254		254
255	// returns the last waypoint	255	// returns the last waypoint
256	Point_t* PointList_WPEnd(void)	256	Point_t* PointList_WPEnd(void)
257	{	257	{
258		258
259	u8 i;	259	u8 i;
260	WPIndex = 0; // set list position invalid	260	WPIndex = 0; // set list position invalid
261	POIIndex = 0; // set invalid	261	POIIndex = 0; // set invalid
262		262
263	if(WPActive == FALSE) return(NULL);	263	if(WPActive == FALSE) return(NULL);
264		264
265	if(PointCount > 0)	265	if(PointCount > 0)
266	{	266	{
267	// search backward!	267	// search backward!
268	for(i = 1; i <= MAX_LIST_LEN; i++)	268	for(i = 1; i <= MAX_LIST_LEN; i++)
269	{	269	{
270	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	270	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
271	{	271	{
272	WPIndex = MAX_LIST_LEN - i + 1;	272	WPIndex = MAX_LIST_LEN - i + 1;
273	break;	273	break;
274	}	274	}
275	}	275	}
276	if(WPIndex) // found a WP within the list	276	if(WPIndex) // found a WP within the list
277	{	277	{
278	NaviData.WaypointIndex = WPCount;	278	NaviData.WaypointIndex = WPCount;
279	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	279	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
280	else POIIndex = 0;	280	else POIIndex = 0;
281	}	281	}
282	else // list contains some points but no WP in the list	282	else // list contains some points but no WP in the list
283	{	283	{
284	// search backward for a POI!	284	// search backward for a POI!
285	for(i = 1; i <= MAX_LIST_LEN; i++)	285	for(i = 1; i <= MAX_LIST_LEN; i++)
286	{	286	{
287	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	287	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
288	{	288	{
289	POIIndex = MAX_LIST_LEN - i + 1;	289	POIIndex = MAX_LIST_LEN - i + 1;
290	break;	290	break;
291	}	291	}
292	}	292	}
293	NaviData.WaypointIndex = 0;	293	NaviData.WaypointIndex = 0;
294	}	294	}
295	}	295	}
296	else // no point in the list	296	else // no point in the list
297	{	297	{
298	POIIndex = 0;	298	POIIndex = 0;
299	NaviData.WaypointIndex = 0;	299	NaviData.WaypointIndex = 0;
300	}	300	}
301	if(WPIndex) return(&(PointList[WPIndex-1]));	301	if(WPIndex) return(&(PointList[WPIndex-1]));
302	else return(NULL);	302	else return(NULL);
303	}	303	}
304		304
305	// returns a pointer to the next waypoint or NULL if the end of the list has been reached	305	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
306	Point_t* PointList_WPNext(void)	306	Point_t* PointList_WPNext(void)
307	{	307	{
308	u8 wp_found = 0;	308	u8 wp_found = 0;
309	if(WPActive == FALSE) return(NULL);	309	if(WPActive == FALSE) return(NULL);
310		310
311	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list	311	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
312	{	312	{
313	u8 i;	313	u8 i;
314	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry	314	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
315	{	315	{
316	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs	316	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
317	{	317	{
318	wp_found = i+1;	318	wp_found = i+1;
319	break;	319	break;
320	}	320	}
321	}	321	}
322	}	322	}
323	if(wp_found)	323	if(wp_found)
324	{	324	{
325	WPIndex = wp_found; // update list position	325	WPIndex = wp_found; // update list position
326	NaviData.WaypointIndex++;	326	NaviData.WaypointIndex++;
327	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	327	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
328	else POIIndex = 0;	328	else POIIndex = 0;
329	return(&(PointList[WPIndex-1])); // return pointer to this waypoint	329	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
330	}	330	}
331	else	331	else
332	{ // no next wp found	332	{ // no next wp found
333	NaviData.WaypointIndex = 0;	333	NaviData.WaypointIndex = 0;
334	POIIndex = 0;	334	POIIndex = 0;
335	return(NULL);	335	return(NULL);
336	}	336	}
337	}	337	}
338		338
339	void PointList_WPActive(u8 set)	339	void PointList_WPActive(u8 set)
340	{	340	{
341	if(set)	341	if(set)
342	{	342	{
343	WPActive = TRUE;	343	WPActive = TRUE;
344	PointList_WPBegin(); // updates POI index	344	PointList_WPBegin(); // updates POI index
345	}	345	}
346	else	346	else
347	{	347	{
348	WPActive = FALSE;	348	WPActive = FALSE;
349	POIIndex = 0; // disable POI also	349	POIIndex = 0; // disable POI also
350	}	350	}
351	}	351	}
352		352
353	Point_t* PointList_GetPOI(void)	353	Point_t* PointList_GetPOI(void)
354	{	354	{
355	return PointList_GetAt(POIIndex);	355	return PointList_GetAt(POIIndex);
356	}	356	}
357		357
358	#define LINE_MAX 70	358	#define LINE_MAX 70
359	#define WP_FILE_VERSION_COMPATIBLE 3	359	#define WP_FILE_VERSION_COMPATIBLE 3
360	// save actual point list to SD card	360	// save actual point list to SD card
361	u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store)	361	u8 PointList_SaveToFile(WPL_Store_t * pWPL_Store)
362	{	362	{
363	File_t *fp;	363	File_t *fp;
364	s8 wpline[LINE_MAX];	364	s8 wpline[LINE_MAX];
365	u8 retval = WPL_ERROR;	365	u8 retval = WPL_ERROR;
366		366
367	if(PointCount == 0) return(WPL_NO_WAYPOINTS);	367	if(PointCount == 0) return(WPL_NO_WAYPOINTS);
368	// user absolute path, i.e. leading /	368	// user absolute path, i.e. leading /
369	if(pWPL_Store->Index == 0)	369	if(pWPL_Store->Index == 0)
370	{	370	{
371	sprintf(wpline, "/default.wpl");	371	sprintf(wpline, "/default.wpl");
372	}	372	}
373	else	373	else
374	{	374	{
375	sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);	375	sprintf(wpline, "/WPL/list_%03d.wpl", pWPL_Store->Index);
376	}	376	}
377	UART1_PutString("\n\r Save WPL...");	377	UART1_PutString("\n\r Save WPL...");
378		378
379	if(Fat16_IsValid())	379	if(Fat16_IsValid())
380	{ // check if wpl file is existing	380	{ // check if wpl file is existing
381	if(fexist_(wpline))	381	if(fexist_(wpline))
382	{ //file is existent	382	{ //file is existent
383	if(!(pWPL_Store->OverwriteFile))	383	if(!(pWPL_Store->OverwriteFile))
384	{	384	{
385	UART1_PutString("Error: file exist!\r\n");	385	UART1_PutString("Error: file exist!\r\n");
386	return(WPL_FILEEXIST);	386	return(WPL_FILEEXIST);
387	}	387	}
388	}	388	}
389	fp = fopen_(wpline, 'w'); // try to open the file	389	fp = fopen_(wpline, 'w'); // try to open the file
390	if(fp == NULL)	390	if(fp == NULL)
391	{	391	{
392	UART1_PutString("ERROR: Creating waypoint file!\r\n");	392	UART1_PutString("ERROR: Creating waypoint file!\r\n");
393	return(retval);	393	return(retval);
394	}	394	}
395	// Create general section and key entries	395	// Create general section and key entries
396	fputs_("[General]\r\n", fp);	396	fputs_("[General]\r\n", fp);
397	sprintf(wpline, "Name=%s\r\n", pWPL_Store->Name);	397	sprintf(wpline, "Name=%s\r\n", pWPL_Store->Name);
398	fputs_(wpline, fp);	398	fputs_(wpline, fp);
399	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);	399	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
400	fputs_(wpline, fp);	400	fputs_(wpline, fp);
401	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);	401	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
402	fputs_(wpline, fp);	402	fputs_(wpline, fp);
403	// dump all points if existent	403	// dump all points if existent
404	if(PointCount)	404	if(PointCount)
405	{	405	{
406	u8 i, u8_1;	406	u8 i, u8_1;
407	s32 i32_1, i32_2;	407	s32 i32_1, i32_2;
408		408
409	for (i = 0; i < PointCount; i++)	409	for (i = 0; i < PointCount; i++)
410	{	410	{
411	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);	411	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
412	fputs_(wpline, fp);	412	fputs_(wpline, fp);
413	// write latitude in deg	413	// write latitude in deg
414	if(PointList[i].Position.Latitude < 0) u8_1 = '-';	414	if(PointList[i].Position.Latitude < 0) u8_1 = '-';
415	else u8_1 = '+';	415	else u8_1 = '+';
416	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;	416	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
417	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;	417	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
418	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);	418	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
419	fputs_(wpline, fp);	419	fputs_(wpline, fp);
420	// write longitude in deg	420	// write longitude in deg
421	if(PointList[i].Position.Longitude < 0) u8_1 = '-';	421	if(PointList[i].Position.Longitude < 0) u8_1 = '-';
422	else u8_1 = '+';	422	else u8_1 = '+';
423	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;	423	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
424	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;	424	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
425	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);	425	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
426	fputs_(wpline, fp);	426	fputs_(wpline, fp);
427	// write tolerace radius in m	427	// write tolerace radius in m
428	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);	428	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
429	fputs_(wpline, fp);	429	fputs_(wpline, fp);
430	// write altitude in m	430	// write altitude in m
431	if(PointList[i].Position.Altitude < 0) u8_1 = '-';	431	if(PointList[i].Position.Altitude < 0) u8_1 = '-';
432	else u8_1 = '+';	432	else u8_1 = '+';
433	if(PointList[i].Type == POINT_TYPE_POI)	433	if(PointList[i].Type == POINT_TYPE_POI)
434	{	434	{
435	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m	435	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m
436	i32_2 = abs(PointList[i].Position.Altitude)%100L;	436	i32_2 = abs(PointList[i].Position.Altitude)%100L;
437	}	437	}
438	else	438	else
439	{	439	{
440	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m	440	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
441	i32_2 = abs(PointList[i].Position.Altitude)%10L;	441	i32_2 = abs(PointList[i].Position.Altitude)%10L;
442	}	442	}
443	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);	443	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);
444	fputs_(wpline, fp);	444	fputs_(wpline, fp);
445	// write climb rate in 0.1 m/s	445	// write climb rate in 0.1 m/s
446	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);	446	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
447	fputs_(wpline, fp);	447	fputs_(wpline, fp);
448	// write hold time in s	448	// write hold time in s
449	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);	449	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
450	fputs_(wpline, fp);	450	fputs_(wpline, fp);
451	// write event channel value	451	// write event channel value
452	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);	452	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
453	fputs_(wpline, fp);	453	fputs_(wpline, fp);
454	// write heading in deg (0= nothing, neg. values index to poi)	454	// write heading in deg (0= nothing, neg. values index to poi)
455	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);	455	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
456	fputs_(wpline, fp);	456	fputs_(wpline, fp);
457	// write speed in 0.1 m/s	457	// write speed in 0.1 m/s
458	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);	458	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
459	fputs_(wpline, fp);	459	fputs_(wpline, fp);
460	// write cam angle in degree (255 -> POI-Automatic)	460	// write cam angle in degree (255 -> POI-Automatic)
461	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);	461	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
462	fputs_(wpline, fp);	462	fputs_(wpline, fp);
463	// write point type	463	// write point type
464	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);	464	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
465	fputs_(wpline, fp);	465	fputs_(wpline, fp);
466	// write prefix	466	// write prefix
467	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);	467	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
468	fputs_(wpline, fp);	468	fputs_(wpline, fp);
469	} // EOF loop over all points	469	} // EOF loop over all points
470	} // EOF if(PointCount)	470	} // EOF if(PointCount)
471	if(EOF == fclose_(fp))	471	if(EOF == fclose_(fp))
472	{	472	{
473	UART1_PutString("failed!\r\n");	473	UART1_PutString("failed!\r\n");
474	}	474	}
475	else	475	else
476	{	476	{
477	UART1_PutString("ok\r\n");	477	UART1_PutString("ok\r\n");
478	retval = WPL_OK;	478	retval = WPL_OK;
479	}	479	}
480	} // EOF if(Fat16_IsValid())	480	} // EOF if(Fat16_IsValid())
481	else	481	else
482	{	482	{
483	UART1_PutString("no file system found!\r\n");	483	UART1_PutString("no file system found!\r\n");
484	retval = WPL_NO_SDCARD_FOUND;	484	retval = WPL_NO_SDCARD_FOUND;
485	}	485	}
486	return(retval);	486	return(retval);
487	}	487	}
488		488
489	// load actual point list from SD card	489	// load actual point list from SD card
490	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)	490	u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store)
491	{	491	{
492	File_t *fp;	492	File_t *fp;
493	s8 wpline[LINE_MAX];	493	s8 wpline[LINE_MAX];
494	u8 retval = WPL_ERROR;	494	u8 retval = WPL_ERROR;
495		495
496	s8 name, value;	496	s8 name, value;
497	u8 i;	497	u8 i;
498		498
499	u8 IsGeneralSection = 0;	499	u8 IsGeneralSection = 0;
500	u8 IsPointSection = 0;	500	u8 IsPointSection = 0;

Subversion Repositories NaviCtrl

(root)/tags/V2.06f/waypoints.c @ 634 - Rev 497 → 498