zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2378 f9c3cf11-9bcb-44bc-f272-b75c42450872
master
jasonshort 14 years ago
parent
16df94168f
commit
b76745470d
4 changed files with 205 additions and 140 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 262
      ArduCopterMega/GCS_Mavlink.pde
  2. 74
      ArduCopterMega/Log.pde
  3. 2
      ArduCopterMega/sensors.pde
  4. 7
      ArduCopterMega/system.pde

262
ArduCopterMega/GCS_Mavlink.pde
Unescape View File

@ -365,6 +365,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -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) @@ -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) @@ -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) @@ -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_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
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_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_NAV_LOITER_TURNS:
case MAV_CMD_CONDITION_CHANGE_ALT:
case MAV_CMD_DO_SET_HOME:
param1 = tell_command.p1;
break;
case MAV_CMD_NAV_TAKEOFF:
param1 = 0;
break;
case MAV_CMD_NAV_LOITER_TIME:
param1 = tell_command.p1; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
param1 = tell_command.lat;
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) @@ -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,80 +592,94 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -565,80 +592,94 @@ 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_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;
}
}
*/
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;
}
}
// 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_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
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_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = 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_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_NAV_LOITER_TURNS:
case MAV_CMD_DO_SET_HOME:
tell_command.p1 = packet.param1;
break;
case MAV_CMD_NAV_TAKEOFF:
tell_command.p1 = 0;
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
tell_command.p1 = packet.param1 * 100;
break;
case MAV_CMD_NAV_LOITER_TIME:
tell_command.p1 = packet.param1; // APM loiter time is in ten second increments
break;
case MAV_CMD_CONDITION_DELAY:
case MAV_CMD_CONDITION_DISTANCE:
tell_command.lat = packet.param1;
break;
case MAV_CMD_DO_JUMP:
tell_command.lat = 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);
@ -646,8 +687,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -646,8 +687,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
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
Unescape View File

@ -78,10 +78,11 @@ print_log_menu(void) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 @@ -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) @@ -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) @@ -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
Unescape View File

@ -56,7 +56,7 @@ void init_barometer(void) @@ -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
Unescape View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { @@ -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() @@ -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) @@ -354,7 +353,7 @@ void set_mode(byte mode)
break;
case RTL:
init_throttle_cruise();
//init_throttle_cruise();
do_RTL();
break;

Write Preview
Loading…
Cancel
Save