Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

2.0.8 - Mavlink fixes. Works much better now with M.Oborne's planner

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2378 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-22 02:49:57 +00:00
parent a1aa95fe6a
commit 8a29d20328
4 changed files with 195 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

242
ArduCopterMega/GCS_Mavlink.pde
View File

@ -365,6 +365,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
//send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
@ -375,13 +376,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// send waypoint
tell_command = get_command_with_index(packet.seq);
// set frame of waypoint
uint8_t frame;
if (tell_command.options & WP_OPTION_ALT_RELATIVE) {
frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame
} else {
@ -392,8 +395,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == g.waypoint_index) current = 1;
if (packet.seq == g.waypoint_index)
current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false)
float x = 0, y = 0, z = 0;
if (tell_command.id < MAV_CMD_NAV_LAST) {
@ -406,47 +413,66 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1*10; // APM loiter time is in ten second increments
break;
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_NAV_TAKEOFF:
param1 = 0;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1; // APM loiter time is in ten second increments
break;
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_JUMP:
param2 = tell_command.lat;
param1 = tell_command.p1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
param4 = tell_command.lng;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
param3 = tell_command.lat;
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_WAYPOINT:
param1 = tell_command.p1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
param2 = tell_command.alt;
param1 = tell_command.p1;
break;
}
mavlink_msg_waypoint_send(chan,msg->sysid,
msg->compid,packet.seq,frame,tell_command.id,current,autocontinue,
param1,param2,param3,param4,x,y,z);
msg->compid,
packet.seq,
frame,
tell_command.id,
current,
autocontinue,
param1,
param2,
param3,
param4,
x,
y,
z);
// update last waypoint comm stamp
waypoint_timelast_send = millis();
@ -546,6 +572,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_WAYPOINT:
{
// Check if receiving waypiont
@ -565,89 +592,102 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// defaults
tell_command.id = packet.command;
switch (packet.frame)
{
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0;
break;
}
/*
switch (packet.frame){
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
break;
}
case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
{
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
break;
}
}
case MAV_FRAME_MISSION:
case MAV_FRAME_GLOBAL:
{
tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7
tell_command.alt = packet.z*1.0e2; // in as m converted to cm
tell_command.options = 0;
break;
}
case MAV_FRAME_LOCAL: // local (relative to home position)
{
tell_command.lat = 1.0e7*ToDeg(packet.x/
(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat;
tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng;
tell_command.alt = packet.z*1.0e2;
tell_command.options = 1;
break;
}
//case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude
default:
{
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = 1; // store altitude relative!! Always!!
break;
}
}
*/
// we only are supporting Abs position, relative Alt
tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7
tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7
tell_command.alt = packet.z * 1.0e2;
tell_command.options = 1; // store altitude relative!! Always!!
switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_DO_SET_HOME:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_NAV_TAKEOFF:
tell_command.p1 = 0;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1 / 10; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1; // APM loiter time is in ten second increments
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
tell_command.lng = packet.param4;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_REPEAT_SERVO:
tell_command.lng = packet.param4;
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CHANGE_SPEED:
tell_command.lat = packet.param3;
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
case MAV_CMD_NAV_WAYPOINT:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_DO_SET_PARAMETER:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_SET_SERVO:
tell_command.alt = packet.param2;
tell_command.p1 = packet.param1;
break;
}
set_command_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
waypoint_request_i++;
if (waypoint_request_i > g.waypoint_total)
{
if (waypoint_request_i > g.waypoint_total){
uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(

74
ArduCopterMega/Log.pde
View File

@ -78,10 +78,11 @@ print_log_menu(void)
Serial.printf_P(PSTR("\nNo logs\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
for(int i = 1; i < last_log_num + 1; i++) {
get_log_boundaries(last_log_num, i, log_start, log_end);
Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"),
i, log_start, log_end);
//Serial.printf_P(PSTR("last_num %d "), last_log_num);
Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"), i, log_start, log_end);
}
Serial.println();
}
@ -98,9 +99,11 @@ dump_log(uint8_t argc, const Menu::arg *argv)
// check that the requested log number can be read
dump_log = argv[1].i;
last_log_num = get_num_logs();
if ((argc != 2) || (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log number\n"));
if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
return(-1);
}
@ -186,11 +189,12 @@ process_logs(uint8_t argc, const Menu::arg *argv)
}
// finds out how many logs are available
byte get_num_logs(void)
{
int page = 1;
byte data;
byte log_step = 0;
byte log_step;
DataFlash.StartRead(1);
@ -211,11 +215,14 @@ byte get_num_logs(void)
break;
case 2:
if(data==LOG_INDEX_MSG){
if(data == LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
//Serial.printf("num_logs, %d\n", num_logs);
return num_logs;
}else{
log_step=0; // Restart, we have a problem...
//Serial.printf("* %d\n", data);
log_step = 0; // Restart, we have a problem...
}
break;
}
@ -225,10 +232,12 @@ byte get_num_logs(void)
}
// send the number of the last log?
void start_new_log(byte num_existing_logs)
void start_new_log()
{
int start_pages[50] = {0, 0, 0};
int end_pages[50] = {0, 0, 0};
byte num_existing_logs = get_num_logs();
int start_pages[50];
int end_pages[50];
if(num_existing_logs > 0){
for(int i = 0; i < num_existing_logs; i++) {
@ -237,7 +246,8 @@ void start_new_log(byte num_existing_logs)
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
if((end_pages[num_existing_logs - 1] < 4095) && (num_existing_logs < MAX_NUM_LOGS /*50*/)) {
if(num_existing_logs > 0)
start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
else
@ -265,38 +275,46 @@ void start_new_log(byte num_existing_logs)
}
}
void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end_page)
// All log data is stored in page 1?
void get_log_boundaries(byte num_logs, byte log_num, int &start_page, int &end_page)
{
int page = 1;
int page = 1;
byte data;
byte log_step = 0;
byte log_step;
// start reading at page 1,
// XXX not 0?
DataFlash.StartRead(page);
DataFlash.StartRead(1);
while (page == 1) {
data = DataFlash.ReadByte();
switch(log_step) //This is a state machine to read the packets
{
switch(log_step){ //This is a state machine to read the packets
case 0:
if(data==HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data==HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data==LOG_INDEX_MSG){
if(data == LOG_INDEX_MSG){
byte num_logs = DataFlash.ReadByte();
for(int i=0;i<log_num;i++) {
start_page = DataFlash.ReadInt();
end_page = DataFlash.ReadInt();
for(int i = 0; i < log_num; i++) {
start_page = DataFlash.ReadInt();
end_page = DataFlash.ReadInt();
//Serial.printf("log %d, start page:%d, end page:%d\n",i, start_page, end_page);
}
if(log_num==num_logs)
// what is this?
//
if(log_num == num_logs)
end_page = find_last_log_page(start_page);
return; // This is the normal exit point
@ -307,9 +325,14 @@ void get_log_boundaries(byte num_logs, byte log_num, int & start_page, int & end
}
page = DataFlash.GetPage();
}
if(page > 1){
Serial.printf("page er:%d\n", page);
}
// Error condition if we reach here with page = 2 TO DO - report condition
}
//
int find_last_log_page(int bottom_page)
{
int top_page = 4096;
@ -320,6 +343,9 @@ int find_last_log_page(int bottom_page)
look_page = (top_page + bottom_page) / 2;
DataFlash.StartRead(look_page);
check = DataFlash.ReadLong();
//Serial.printf("look page:%d, check:%d\n", look_page, check);
if(check == 0xFFFFFFFF)
top_page = look_page;
else
@ -614,15 +640,15 @@ void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteByte(wp->options);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
}
//CMD, 3, 0, 16, 8, 1, 800, 340440192, -1180692736
// Read a command processing packet

2
ArduCopterMega/sensors.pde
View File

@ -56,7 +56,7 @@ void init_barometer(void)
ground_pressure = abs_pressure;
//Serial.printf("abs_pressure %ld\n", abs_pressure);
SendDebugln("barometer calibration complete.");
//SendDebugln("barometer calibration complete.");
}
long read_barometer(void)

7
ArduCopterMega/system.pde
View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "AC 2.0.7 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.8 Beta", main_menu_commands);
void init_ardupilot()
{
@ -239,8 +239,7 @@ void init_ardupilot()
if(g.log_bitmask != 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
byte last_log_num = get_num_logs();
start_new_log(last_log_num);
start_new_log();
}
if (g.log_bitmask & MASK_LOG_MODE)
@ -354,7 +353,7 @@ void set_mode(byte mode)
break;
case RTL:
init_throttle_cruise();
//init_throttle_cruise();
do_RTL();
break;