mirror of https://github.com/ArduPilot/ardupilot
106 lines
2.3 KiB
Plaintext
106 lines
2.3 KiB
Plaintext
|
|
void writePoints()
|
|
{
|
|
int mem;
|
|
struct Location loc;
|
|
|
|
|
|
for (byte i = 0; i < waypoint_total; i++){
|
|
|
|
loc.id = (uint8_t) mission[i][0];
|
|
loc.options = (uint8_t) mission[i][1];
|
|
loc.p1 = (uint8_t) mission[i][2];
|
|
loc.alt = (long)(mission[i][3] * 100);
|
|
loc.lat = (long)(mission[i][4] * t7);
|
|
loc.lng = (long)(mission[i][5] * t7);
|
|
|
|
switch(loc.id){
|
|
case MAV_CMD_NAV_WAYPOINT:
|
|
//loc.p1 = (byte)mission[i][4];// wp_radius
|
|
//loc.p1 = WP_RADIUS;
|
|
break;
|
|
|
|
case MAV_CMD_CONDITION_YAW:
|
|
loc.alt = (long)mission[i][3]; // speed
|
|
loc.lat = (long)mission[i][4]; // rotation direction
|
|
loc.lng = (long)mission[i][5]; // target yaw in deg
|
|
break;
|
|
}
|
|
|
|
set_wp_with_index(loc,(i+1));
|
|
|
|
/*
|
|
Serial.print((i+1),DEC);
|
|
Serial.print(": ");
|
|
Serial.print(loc.id,DEC);
|
|
Serial.print(", ");
|
|
Serial.print(loc.p1,DEC);
|
|
Serial.print(", ");
|
|
Serial.print(loc.alt,DEC);
|
|
Serial.print(", ");
|
|
Serial.print(loc.lat,DEC);
|
|
Serial.print(", ");
|
|
Serial.println(loc.lng,DEC);
|
|
*/
|
|
}
|
|
}
|
|
|
|
|
|
struct Location get_wp_with_index(int i)
|
|
{
|
|
struct Location temp;
|
|
long mem;
|
|
|
|
// Find out proper location in memory by using the start_byte position + the index
|
|
// --------------------------------------------------------------------------------
|
|
if (i > waypoint_total) {
|
|
temp.id = CMD_BLANK;
|
|
}else{
|
|
// read WP position
|
|
mem = (WP_START_BYTE) + (i * WP_SIZE);
|
|
temp.id = eeprom_read_byte((uint8_t*)mem);
|
|
|
|
mem++;
|
|
temp.options = eeprom_read_byte((uint8_t*)mem);
|
|
|
|
mem++;
|
|
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
|
|
|
mem++;
|
|
temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM!
|
|
|
|
mem += 4;
|
|
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
|
|
|
|
mem += 4;
|
|
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
|
|
}
|
|
return temp;
|
|
}
|
|
|
|
void set_wp_with_index(struct Location temp, int i)
|
|
{
|
|
i = constrain(i, 0, waypoint_total.get());
|
|
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.id);
|
|
|
|
mem++;
|
|
eeprom_write_byte((uint8_t *) mem, temp.options);
|
|
|
|
mem++;
|
|
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
|
|
|
mem++;
|
|
eeprom_write_dword((uint32_t *) mem, temp.alt); // alt is stored in CM!
|
|
|
|
mem += 4;
|
|
eeprom_write_dword((uint32_t *) mem, temp.lat);
|
|
|
|
mem += 4;
|
|
eeprom_write_dword((uint32_t *) mem, temp.lng);
|
|
}
|
|
|
|
|
|
|