WebSVN - NaviCtrl - Diff - Rev 492 and 494


/*#######################################################################################*/
/* !!! 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"
 
// 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(u8 place, s8 * name)
{
        File_t *fp;
        s8 wpline[LINE_MAX], retval = 0;
        // user absolute path, i.e. leading /
 
        sprintf(wpline, "/list_%03d.wpl", place);
 
        UART1_PutString("\n\r Write ");
        UART1_PutString(wpline);
        UART1_PutString("...");
 
        UART1_PutString("\n\r Save WPL...");
 
        if(Fat16_IsValid())
        {       // check if wpl file is existing
                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", 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 
                                //if(PointList[i].Type == POINT_TYPE_WP) u8_1 = 'P';
                                //else u8_1 = '0';
                                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 = 1;
                }                                
        } // EOF if(Fat16_IsValid())
        else UART1_PutString("no file system found!\r\n");     
        return(retval);
}
 
// load actual point list from SD card
u8 PointList_ReadFromFile(u8 place)
{
        File_t *fp;
        s8 wpline[LINE_MAX], retval = 0;
       
        s8 *name, *value;
        u8 i;
 
        u8 IsGeneralSection = 0;
        u8 IsPointSection  = 0;
        u8 WPNumber = 0;
 
        // user absolute path, i.e. leading /
        sprintf(wpline, "/list_%03d.wpl", place);
 
        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(0);
                }
                // clear point list first
                PointList_Clear();
                // 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(1); // we are done here  
                                                                }
                                                                else if(WPNumber > MAX_LIST_LEN) // number o points larger than ram list
                                                                {
                                                                        UART1_PutString("To much points!");
                                                                        return(0);
                                                                }
                                                        }
                                                        else if (strcmp(name, "FILEVERSION") == 0)
                                                        {
                                                                if((u8)atoi(value) != WP_FILE_VERSION_COMPATIBLE)
                                                                {
                                                                        UART1_PutString("Bad file version!\r\n");
                                                                        return(0);     
                                                                }
                                                        }
                                                        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;
                UART1_PutString("ok\r\n");
                retval = 1;                              
        } // EOF if(Fat16_IsValid())
        else UART1_PutString("no file system found!\r\n");     
        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 492	Rev 494
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	// the waypoints list	65	// the waypoints list
66	#define MAX_LIST_LEN 101	66	#define MAX_LIST_LEN 101
67		67
68	Point_t PointList[MAX_LIST_LEN];	68	Point_t PointList[MAX_LIST_LEN];
69	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount	69	u8 WPIndex = 0; // list index of GPS point representig the current WP, can be maximal WPCount
70	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount	70	u8 POIIndex = 0; // list index of GPS Point representing the current POI, can be maximal WPCount
71	u8 WPCount = 0; // number of waypoints	71	u8 WPCount = 0; // number of waypoints
72	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN	72	u8 PointCount = 0; // number of points in the list can be maximal equal to MAX_LIST_LEN
73	u8 POICount = 0; // number of point of interest in the list	73	u8 POICount = 0; // number of point of interest in the list
74		74
75	u8 WPActive = FALSE;	75	u8 WPActive = FALSE;
76		76
77	u8 PointList_Init(void)	77	u8 PointList_Init(void)
78	{	78	{
79	return PointList_Clear();	79	return PointList_Clear();
80	}	80	}
81		81
82	u8 PointList_Clear(void)	82	u8 PointList_Clear(void)
83	{	83	{
84	u8 i;	84	u8 i;
85	WPIndex = 0; // real list position are 1 ,2, 3 ...	85	WPIndex = 0; // real list position are 1 ,2, 3 ...
86	POIIndex = 0; // real list position are 1 ,2, 3 ...	86	POIIndex = 0; // real list position are 1 ,2, 3 ...
87	WPCount = 0; // no waypoints	87	WPCount = 0; // no waypoints
88	POICount = 0;	88	POICount = 0;
89	PointCount = 0; // no contents	89	PointCount = 0; // no contents
90	WPActive = FALSE;	90	WPActive = FALSE;
91	NaviData.WaypointNumber = WPCount;	91	NaviData.WaypointNumber = WPCount;
92	NaviData.WaypointIndex = 0;	92	NaviData.WaypointIndex = 0;
93		93
94	for(i = 0; i < MAX_LIST_LEN; i++)	94	for(i = 0; i < MAX_LIST_LEN; i++)
95	{	95	{
96	PointList[i].Position.Status = INVALID;	96	PointList[i].Position.Status = INVALID;
97	PointList[i].Position.Latitude = 0;	97	PointList[i].Position.Latitude = 0;
98	PointList[i].Position.Longitude = 0;	98	PointList[i].Position.Longitude = 0;
99	PointList[i].Position.Altitude = 0;	99	PointList[i].Position.Altitude = 0;
100	PointList[i].Heading = 361; // invalid value	100	PointList[i].Heading = 361; // invalid value
101	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered	101	PointList[i].ToleranceRadius = 0; // in meters, if the MK is within that range around the target, then the next target is triggered
102	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	102	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
103	PointList[i].Event_Flag = 0; // future implementation	103	PointList[i].Event_Flag = 0; // future implementation
104	PointList[i].Index = 0;	104	PointList[i].Index = 0;
105	PointList[i].Type = POINT_TYPE_INVALID;	105	PointList[i].Type = POINT_TYPE_INVALID;
106	PointList[i].WP_EventChannelValue = 0;	106	PointList[i].WP_EventChannelValue = 0;
107	PointList[i].AltitudeRate = 0; // no change of setpoint	107	PointList[i].AltitudeRate = 0; // no change of setpoint
108	PointList[i].Speed = 0;	108	PointList[i].Speed = 0;
109	PointList[i].CamAngle = 0;	109	PointList[i].CamAngle = 0;
110	PointList[i].Name[0] = 0;	110	PointList[i].Name[0] = 0;
111	}	111	}
112	return TRUE;	112	return TRUE;
113	}	113	}
114		114
115	u8 PointList_GetCount(void)	115	u8 PointList_GetCount(void)
116	{	116	{
117	return PointCount; // number of points in the list	117	return PointCount; // number of points in the list
118	}	118	}
119		119
120	Point_t* PointList_GetAt(u8 index)	120	Point_t* PointList_GetAt(u8 index)
121	{	121	{
122	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint	122	if((index > 0) && (index <= PointCount)) return(&(PointList[index-1])); // return pointer to this waypoint
123	else return(NULL);	123	else return(NULL);
124	}	124	}
125		125
126	u8 PointList_SetAt(Point_t* pPoint)	126	u8 PointList_SetAt(Point_t* pPoint)
127	{	127	{
128	// if index is in range	128	// if index is in range
129	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))	129	if((pPoint->Index > 0) && (pPoint->Index <= MAX_LIST_LEN))
130	{	130	{
131	// check list entry before update	131	// check list entry before update
132	switch(PointList[pPoint->Index-1].Type)	132	switch(PointList[pPoint->Index-1].Type)
133	{	133	{
134	case POINT_TYPE_INVALID: // was invalid	134	case POINT_TYPE_INVALID: // was invalid
135	switch(pPoint->Type)	135	switch(pPoint->Type)
136	{	136	{
137	default:	137	default:
138	case POINT_TYPE_INVALID:	138	case POINT_TYPE_INVALID:
139	// nothing to do	139	// nothing to do
140	break;	140	break;
141		141
142	case POINT_TYPE_WP:	142	case POINT_TYPE_WP:
143	WPCount++;	143	WPCount++;
144	PointCount++;	144	PointCount++;
145	break;	145	break;
146		146
147	case POINT_TYPE_POI:	147	case POINT_TYPE_POI:
148	POICount++;	148	POICount++;
149	PointCount++;	149	PointCount++;
150	break;	150	break;
151	}	151	}
152	break;	152	break;
153		153
154	case POINT_TYPE_WP: // was a waypoint	154	case POINT_TYPE_WP: // was a waypoint
155	switch(pPoint->Type)	155	switch(pPoint->Type)
156	{	156	{
157	case POINT_TYPE_INVALID:	157	case POINT_TYPE_INVALID:
158	WPCount--;	158	WPCount--;
159	PointCount--;	159	PointCount--;
160	break;	160	break;
161		161
162	default:	162	default:
163	case POINT_TYPE_WP:	163	case POINT_TYPE_WP:
164	//nothing to do	164	//nothing to do
165	break;	165	break;
166		166
167	case POINT_TYPE_POI:	167	case POINT_TYPE_POI:
168	POICount++;	168	POICount++;
169	WPCount--;	169	WPCount--;
170	break;	170	break;
171	}	171	}
172	break;	172	break;
173		173
174	case POINT_TYPE_POI: // was a poi	174	case POINT_TYPE_POI: // was a poi
175	switch(pPoint->Type)	175	switch(pPoint->Type)
176	{	176	{
177	case POINT_TYPE_INVALID:	177	case POINT_TYPE_INVALID:
178	POICount--;	178	POICount--;
179	PointCount--;	179	PointCount--;
180	break;	180	break;
181		181
182	case POINT_TYPE_WP:	182	case POINT_TYPE_WP:
183	WPCount++;	183	WPCount++;
184	POICount--;	184	POICount--;
185	break;	185	break;
186		186
187	case POINT_TYPE_POI:	187	case POINT_TYPE_POI:
188	default:	188	default:
189	// nothing to do	189	// nothing to do
190	break;	190	break;
191	}	191	}
192	break;	192	break;
193	}	193	}
194	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry	194	memcpy(&PointList[pPoint->Index-1], pPoint, sizeof(Point_t)); // copy data to list entry
195	NaviData.WaypointNumber = WPCount;	195	NaviData.WaypointNumber = WPCount;
196	return pPoint->Index;	196	return pPoint->Index;
197	}	197	}
198	else return(0);	198	else return(0);
199	}	199	}
200		200
201	// returns the pointer to the first waypoint within the list	201	// returns the pointer to the first waypoint within the list
202	Point_t* PointList_WPBegin(void)	202	Point_t* PointList_WPBegin(void)
203	{	203	{
204	u8 i;	204	u8 i;
205	WPIndex = 0; // set list position invalid	205	WPIndex = 0; // set list position invalid
206		206
207	if(WPActive == FALSE) return(NULL);	207	if(WPActive == FALSE) return(NULL);
208		208
209	POIIndex = 0; // set invalid POI	209	POIIndex = 0; // set invalid POI
210	if(PointCount > 0)	210	if(PointCount > 0)
211	{	211	{
212	// search for first wp in list	212	// search for first wp in list
213	for(i = 0; i <MAX_LIST_LEN; i++)	213	for(i = 0; i <MAX_LIST_LEN; i++)
214	{	214	{
215	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))	215	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID))
216	{	216	{
217	WPIndex = i + 1;	217	WPIndex = i + 1;
218	break;	218	break;
219	}	219	}
220	}	220	}
221	if(WPIndex) // found a WP in the list	221	if(WPIndex) // found a WP in the list
222	{	222	{
223	NaviData.WaypointIndex = 1;	223	NaviData.WaypointIndex = 1;
224	// update index to POI	224	// update index to POI
225	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	225	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
226	else POIIndex = 0;	226	else POIIndex = 0;
227	}	227	}
228	else // some points in the list but no WP found	228	else // some points in the list but no WP found
229	{	229	{
230	NaviData.WaypointIndex = 0;	230	NaviData.WaypointIndex = 0;
231	//Check for an existing POI	231	//Check for an existing POI
232	for(i = 0; i < MAX_LIST_LEN; i++)	232	for(i = 0; i < MAX_LIST_LEN; i++)
233	{	233	{
234	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))	234	if((PointList[i].Type == POINT_TYPE_POI) && (PointList[i].Position.Status != INVALID))
235	{	235	{
236	POIIndex = i + 1;	236	POIIndex = i + 1;
237	break;	237	break;
238	}	238	}
239	}	239	}
240	}	240	}
241	}	241	}
242	else // no point in the list	242	else // no point in the list
243	{	243	{
244	POIIndex = 0;	244	POIIndex = 0;
245	NaviData.WaypointIndex = 0;	245	NaviData.WaypointIndex = 0;
246	}	246	}
247		247
248	if(WPIndex) return(&(PointList[WPIndex-1]));	248	if(WPIndex) return(&(PointList[WPIndex-1]));
249	else return(NULL);	249	else return(NULL);
250	}	250	}
251		251
252	// returns the last waypoint	252	// returns the last waypoint
253	Point_t* PointList_WPEnd(void)	253	Point_t* PointList_WPEnd(void)
254	{	254	{
255		255
256	u8 i;	256	u8 i;
257	WPIndex = 0; // set list position invalid	257	WPIndex = 0; // set list position invalid
258	POIIndex = 0; // set invalid	258	POIIndex = 0; // set invalid
259		259
260	if(WPActive == FALSE) return(NULL);	260	if(WPActive == FALSE) return(NULL);
261		261
262	if(PointCount > 0)	262	if(PointCount > 0)
263	{	263	{
264	// search backward!	264	// search backward!
265	for(i = 1; i <= MAX_LIST_LEN; i++)	265	for(i = 1; i <= MAX_LIST_LEN; i++)
266	{	266	{
267	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	267	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_WP) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
268	{	268	{
269	WPIndex = MAX_LIST_LEN - i + 1;	269	WPIndex = MAX_LIST_LEN - i + 1;
270	break;	270	break;
271	}	271	}
272	}	272	}
273	if(WPIndex) // found a WP within the list	273	if(WPIndex) // found a WP within the list
274	{	274	{
275	NaviData.WaypointIndex = WPCount;	275	NaviData.WaypointIndex = WPCount;
276	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	276	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
277	else POIIndex = 0;	277	else POIIndex = 0;
278	}	278	}
279	else // list contains some points but no WP in the list	279	else // list contains some points but no WP in the list
280	{	280	{
281	// search backward for a POI!	281	// search backward for a POI!
282	for(i = 1; i <= MAX_LIST_LEN; i++)	282	for(i = 1; i <= MAX_LIST_LEN; i++)
283	{	283	{
284	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))	284	if((PointList[MAX_LIST_LEN - i].Type == POINT_TYPE_POI) && (PointList[MAX_LIST_LEN - i].Position.Status != INVALID))
285	{	285	{
286	POIIndex = MAX_LIST_LEN - i + 1;	286	POIIndex = MAX_LIST_LEN - i + 1;
287	break;	287	break;
288	}	288	}
289	}	289	}
290	NaviData.WaypointIndex = 0;	290	NaviData.WaypointIndex = 0;
291	}	291	}
292	}	292	}
293	else // no point in the list	293	else // no point in the list
294	{	294	{
295	POIIndex = 0;	295	POIIndex = 0;
296	NaviData.WaypointIndex = 0;	296	NaviData.WaypointIndex = 0;
297	}	297	}
298	if(WPIndex) return(&(PointList[WPIndex-1]));	298	if(WPIndex) return(&(PointList[WPIndex-1]));
299	else return(NULL);	299	else return(NULL);
300	}	300	}
301		301
302	// returns a pointer to the next waypoint or NULL if the end of the list has been reached	302	// returns a pointer to the next waypoint or NULL if the end of the list has been reached
303	Point_t* PointList_WPNext(void)	303	Point_t* PointList_WPNext(void)
304	{	304	{
305	u8 wp_found = 0;	305	u8 wp_found = 0;
306	if(WPActive == FALSE) return(NULL);	306	if(WPActive == FALSE) return(NULL);
307		307
308	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list	308	if(WPIndex < MAX_LIST_LEN) // if there is a next entry in the list
309	{	309	{
310	u8 i;	310	u8 i;
311	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry	311	for(i = WPIndex; i < MAX_LIST_LEN; i++) // start search for next at next list entry
312	{	312	{
313	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs	313	if((PointList[i].Type == POINT_TYPE_WP) && (PointList[i].Position.Status != INVALID)) // jump over POIs
314	{	314	{
315	wp_found = i+1;	315	wp_found = i+1;
316	break;	316	break;
317	}	317	}
318	}	318	}
319	}	319	}
320	if(wp_found)	320	if(wp_found)
321	{	321	{
322	WPIndex = wp_found; // update list position	322	WPIndex = wp_found; // update list position
323	NaviData.WaypointIndex++;	323	NaviData.WaypointIndex++;
324	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);	324	if(PointList[WPIndex-1].Heading < 0) POIIndex = (u8)(-PointList[WPIndex-1].Heading);
325	else POIIndex = 0;	325	else POIIndex = 0;
326	return(&(PointList[WPIndex-1])); // return pointer to this waypoint	326	return(&(PointList[WPIndex-1])); // return pointer to this waypoint
327	}	327	}
328	else	328	else
329	{ // no next wp found	329	{ // no next wp found
330	NaviData.WaypointIndex = 0;	330	NaviData.WaypointIndex = 0;
331	POIIndex = 0;	331	POIIndex = 0;
332	return(NULL);	332	return(NULL);
333	}	333	}
334	}	334	}
335		335
336	void PointList_WPActive(u8 set)	336	void PointList_WPActive(u8 set)
337	{	337	{
338	if(set)	338	if(set)
339	{	339	{
340	WPActive = TRUE;	340	WPActive = TRUE;
341	PointList_WPBegin(); // updates POI index	341	PointList_WPBegin(); // updates POI index
342	}	342	}
343	else	343	else
344	{	344	{
345	WPActive = FALSE;	345	WPActive = FALSE;
346	POIIndex = 0; // disable POI also	346	POIIndex = 0; // disable POI also
347	}	347	}
348	}	348	}
349		349
350	Point_t* PointList_GetPOI(void)	350	Point_t* PointList_GetPOI(void)
351	{	351	{
352	return PointList_GetAt(POIIndex);	352	return PointList_GetAt(POIIndex);
353	}	353	}
354		354
355	#define LINE_MAX 70	355	#define LINE_MAX 70
356	#define WP_FILE_VERSION_COMPATIBLE 3	356	#define WP_FILE_VERSION_COMPATIBLE 3
357	// save actual point list to SD card	357	// save actual point list to SD card
358	u8 PointList_SaveToFile(u8 place)	358	u8 PointList_SaveToFile(u8 place, s8 * name)
359	{	359	{
360	File_t *fp;	360	File_t *fp;
361	s8 wpline[LINE_MAX], retval = 0;	361	s8 wpline[LINE_MAX], retval = 0;
362	// user absolute path, i.e. leading /	362	// user absolute path, i.e. leading /
363		363
364	sprintf(wpline, "/list_%03d.wpl", place);	364	sprintf(wpline, "/list_%03d.wpl", place);
365		365
366	UART1_PutString("\n\r Write ");	366	UART1_PutString("\n\r Write ");
367	UART1_PutString(wpline);	367	UART1_PutString(wpline);
368	UART1_PutString("...");	368	UART1_PutString("...");
369		369
370	UART1_PutString("\n\r Save WPL...");	370	UART1_PutString("\n\r Save WPL...");
371		371
372	if(Fat16_IsValid())	372	if(Fat16_IsValid())
373	{ // check if wpl file is existing	373	{ // check if wpl file is existing
374	fp = fopen_(wpline, 'w'); // try to open the file	374	fp = fopen_(wpline, 'w'); // try to open the file
375	if(fp == NULL)	375	if(fp == NULL)
376	{	376	{
377	UART1_PutString("ERROR: Creating waypoint file!\r\n");	377	UART1_PutString("ERROR: Creating waypoint file!\r\n");
378	return(retval);	378	return(retval);
379	}	379	}
380	// Create general section and key entries	380	// Create general section and key entries
381	fputs_("[General]\r\n", fp);	381	fputs_("[General]\r\n", fp);
-		382	sprintf(wpline, "Name=%s\r\n", name);
-		383	fputs_(wpline, fp);
382	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);	384	sprintf(wpline, "FileVersion=%d\r\n", WP_FILE_VERSION_COMPATIBLE);
383	fputs_(wpline, fp);	385	fputs_(wpline, fp);
384	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);	386	sprintf(wpline, "NumberOfWaypoints=%d\r\n", PointCount);
385	fputs_(wpline, fp);	387	fputs_(wpline, fp);
386	// dump all points if existent	388	// dump all points if existent
387	if(PointCount)	389	if(PointCount)
388	{	390	{
389	u8 i, u8_1;	391	u8 i, u8_1;
390	s32 i32_1, i32_2;	392	s32 i32_1, i32_2;
391		393
392	for (i = 0; i < PointCount; i++)	394	for (i = 0; i < PointCount; i++)
393	{	395	{
394	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);	396	sprintf(wpline, "[Point%d]\r\n",PointList[i].Index);
395	fputs_(wpline, fp);	397	fputs_(wpline, fp);
396	// write latitude in deg	398	// write latitude in deg
397	if(PointList[i].Position.Latitude < 0) u8_1 = '-';	399	if(PointList[i].Position.Latitude < 0) u8_1 = '-';
398	else u8_1 = '+';	400	else u8_1 = '+';
399	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;	401	i32_1 = abs(PointList[i].Position.Latitude)/10000000L;
400	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;	402	i32_2 = abs(PointList[i].Position.Latitude)%10000000L;
401	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);	403	sprintf(wpline, "Latitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
402	fputs_(wpline, fp);	404	fputs_(wpline, fp);
403	// write longitude in deg	405	// write longitude in deg
404	if(PointList[i].Position.Longitude < 0) u8_1 = '-';	406	if(PointList[i].Position.Longitude < 0) u8_1 = '-';
405	else u8_1 = '+';	407	else u8_1 = '+';
406	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;	408	i32_1 = abs(PointList[i].Position.Longitude)/10000000L;
407	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;	409	i32_2 = abs(PointList[i].Position.Longitude)%10000000L;
408	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);	410	sprintf(wpline, "Longitude=%c%ld.%07ld\r\n", u8_1, i32_1, i32_2);
409	fputs_(wpline, fp);	411	fputs_(wpline, fp);
410	// write tolerace radius in m	412	// write tolerace radius in m
411	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);	413	sprintf(wpline, "Radius=%d\r\n", PointList[i].ToleranceRadius);
412	fputs_(wpline, fp);	414	fputs_(wpline, fp);
413	// write altitude in m	415	// write altitude in m
414	if(PointList[i].Position.Altitude < 0) u8_1 = '-';	416	if(PointList[i].Position.Altitude < 0) u8_1 = '-';
415	else u8_1 = '+';	417	else u8_1 = '+';
416	if(PointList[i].Type == POINT_TYPE_POI)	418	if(PointList[i].Type == POINT_TYPE_POI)
417	{	419	{
418	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m	420	i32_1 = abs(PointList[i].Position.Altitude)/100L; // cm --> m
419	i32_2 = abs(PointList[i].Position.Altitude)%100L;	421	i32_2 = abs(PointList[i].Position.Altitude)%100L;
420	}	422	}
421	else	423	else
422	{	424	{
423	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m	425	i32_1 = abs(PointList[i].Position.Altitude)/10L; // dm --> m
424	i32_2 = abs(PointList[i].Position.Altitude)%10L;	426	i32_2 = abs(PointList[i].Position.Altitude)%10L;
425	}	427	}
426	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);	428	sprintf(wpline, "Altitude=%c%ld.%01ld\r\n", u8_1, i32_1, i32_2);
427	fputs_(wpline, fp);	429	fputs_(wpline, fp);
428	// write climb rate in 0.1 m/s	430	// write climb rate in 0.1 m/s
429	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);	431	sprintf(wpline, "ClimbRate=%d\r\n", PointList[i].AltitudeRate);
430	fputs_(wpline, fp);	432	fputs_(wpline, fp);
431	// write hold time in s	433	// write hold time in s
432	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);	434	sprintf(wpline, "DelayTime=%d\r\n", PointList[i].HoldTime);
433	fputs_(wpline, fp);	435	fputs_(wpline, fp);
434	// write event channel value	436	// write event channel value
435	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);	437	sprintf(wpline, "WP_Event_Channel_Value=%d\r\n", PointList[i].WP_EventChannelValue);
436	fputs_(wpline, fp);	438	fputs_(wpline, fp);
437	// write heading in deg (0= nothing, neg. values index to poi)	439	// write heading in deg (0= nothing, neg. values index to poi)
438	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);	440	sprintf(wpline, "Heading=%d\r\n", PointList[i].Heading);
439	fputs_(wpline, fp);	441	fputs_(wpline, fp);
440	// write speed in 0.1 m/s	442	// write speed in 0.1 m/s
441	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);	443	sprintf(wpline, "Speed=%d\r\n", PointList[i].Speed);
442	fputs_(wpline, fp);	444	fputs_(wpline, fp);
443	// write cam angle in degree (255 -> POI-Automatic)	445	// write cam angle in degree (255 -> POI-Automatic)
444	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);	446	sprintf(wpline, "CAM-Nick=%d\r\n", PointList[i].CamAngle);
445	fputs_(wpline, fp);	447	fputs_(wpline, fp);
446	// write point type	448	// write point type
447	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);	449	sprintf(wpline, "Type=%d\r\n", PointList[i].Type + 1);
448	fputs_(wpline, fp);	450	fputs_(wpline, fp);
449	// write prefix	451	// write prefix
450	//if(PointList[i].Type == POINT_TYPE_WP) u8_1 = 'P';	452	//if(PointList[i].Type == POINT_TYPE_WP) u8_1 = 'P';
451	//else u8_1 = '0';	453	//else u8_1 = '0';
452	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);	454	sprintf(wpline, "Prefix=%s\r\n", PointList[i].Name);
453	fputs_(wpline, fp);	455	fputs_(wpline, fp);
454	} // EOF loop over all points	456	} // EOF loop over all points
455	} // EOF if(PointCount)	457	} // EOF if(PointCount)
456	if(EOF == fclose_(fp))	458	if(EOF == fclose_(fp))
457	{	459	{
458	UART1_PutString("failed!\r\n");	460	UART1_PutString("failed!\r\n");
459	}	461	}
460	else	462	else
461	{	463	{
462	UART1_PutString("ok\r\n");	464	UART1_PutString("ok\r\n");
463	retval = 1;	465	retval = 1;
464	}	466	}
465	} // EOF if(Fat16_IsValid())	467	} // EOF if(Fat16_IsValid())
466	else UART1_PutString("no file system found!\r\n");	468	else UART1_PutString("no file system found!\r\n");
467	return(retval);	469	return(retval);
468	}	470	}
469		471
470	// load actual point list from SD card	472	// load actual point list from SD card
471	u8 PointList_ReadFromFile(u8 place)	473	u8 PointList_ReadFromFile(u8 place)
472	{	474	{
473	File_t *fp;	475	File_t *fp;
474	s8 wpline[LINE_MAX], retval = 0;	476	s8 wpline[LINE_MAX], retval = 0;
475		477
476	s8 name, value;	478	s8 name, value;
477	u8 i;	479	u8 i;
478		480
479	u8 IsGeneralSection = 0;	481	u8 IsGeneralSection = 0;
480	u8 IsPointSection = 0;	482	u8 IsPointSection = 0;
481	u8 WPNumber = 0;	483	u8 WPNumber = 0;
482		484
483	// user absolute path, i.e. leading /	485	// user absolute path, i.e. leading /
484	sprintf(wpline, "/list_%03d.wpl", place);	486	sprintf(wpline, "/list_%03d.wpl", place);
485		487
486	UART1_PutString("\n\r Read ");	488	UART1_PutString("\n\r Read ");
487	UART1_PutString(wpline);	489	UART1_PutString(wpline);
488	UART1_PutString("...");	490	UART1_PutString("...");
489		491
490	if(Fat16_IsValid())	492	if(Fat16_IsValid())
491	{ // check if wpl file is existing	493	{ // check if wpl file is existing
492	fp = fopen_(wpline, 'r'); // try to open the file	494	fp = fopen_(wpline, 'r'); // try to open the file
493	if(fp == NULL)	495	if(fp == NULL)
494	{	496	{
495	UART1_PutString("ERROR: Reading waypoint file!\r\n");	497	UART1_PutString("ERROR: Reading waypoint file!\r\n");
496	return(0);	498	return(0);
497	}	499	}
498	// clear point list first	500	// clear point list first

Subversion Repositories NaviCtrl

(root)/tags/V2.16a/waypoints.c @ 934 - Rev 492 → 494