From b76745470df8260349dd01bd8b8e77540bdf009a Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 22 May 2011 02:49:57 +0000 Subject: [PATCH] 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 --- ArduCopterMega/GCS_Mavlink.pde | 242 +++++++++++++++++++-------------- ArduCopterMega/Log.pde | 74 ++++++---- ArduCopterMega/sensors.pde | 2 +- ArduCopterMega/system.pde | 7 +- 4 files changed, 195 insertions(+), 130 deletions(-) diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index eb3b6773f8..86779fdd7f 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -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( diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 4ebb5c34dc..3c88dac0e1 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -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 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 diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index 7fa9da899b..b48cc7b073 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -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) diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index bf8b08348b..5ce2d5fb0a 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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;