From 5c4bab3941ce73b3b876315171e6122f656bc0c4 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 9 May 2011 17:40:32 +0000 Subject: [PATCH] Mavlink Sync with APM git-svn-id: https://arducopter.googlecode.com/svn/trunk@2235 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 14 +++------ ArduCopterMega/GCS.h | 4 +++ ArduCopterMega/GCS_Mavlink.pde | 24 ++++++++------ ArduCopterMega/Log.pde | 52 +++++++++++++++++-------------- ArduCopterMega/Mavlink_Common.h | 24 ++++++++++++-- ArduCopterMega/defines.h | 2 ++ ArduCopterMega/system.pde | 13 ++++---- ArduCopterMega/test.pde | 2 +- 8 files changed, 84 insertions(+), 51 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index d46bf9d363..15030c29ae 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -43,9 +43,6 @@ version 2.1 of the License, or (at your option) any later version. #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions - - - // Configuration #include "config.h" @@ -75,10 +72,12 @@ FastSerialPort3(Serial3); // Telemetry port // Parameters g; + //////////////////////////////////////////////////////////////////////////////// // prototypes void update_events(void); + //////////////////////////////////////////////////////////////////////////////// // Sensors //////////////////////////////////////////////////////////////////////////////// @@ -175,6 +174,7 @@ GPS *g_gps; //////////////////////////////////////////////////////////////////////////////// // #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK + //GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); #else // If we are not using a GCS, we need a stub that does nothing. @@ -300,7 +300,6 @@ float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter -float current_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter float current_amps; float current_total; @@ -411,10 +410,6 @@ float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm) long perf_mon_timer; float imu_health; // Metric based on accel gain deweighting int G_Dt_max; // Max main loop cycle time in milliseconds -byte gyro_sat_count; -byte adc_constraints; -byte renorm_sqrt_count; -byte renorm_blowup_count; int gps_fix_count; byte gcs_messages_sent; @@ -1284,4 +1279,5 @@ void tuning(){ g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 #endif -} +} + diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h index fc7a141886..940522986f 100644 --- a/ArduCopterMega/GCS.h +++ b/ArduCopterMega/GCS.h @@ -66,6 +66,7 @@ public: void send_text(uint8_t severity, const char *str) {} #define send_text_P(severity, msg) send_text(severity, msg) + /// Send a text message with a PSTR() /// /// @param severity A value describing the importance of the message. @@ -196,6 +197,9 @@ private: AP_Int16 streamRateExtra1; AP_Int16 streamRateExtra2; AP_Int16 streamRateExtra3; + + + }; #endif // GCS_PROTOCOL_MAVLINK diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index cec84de0ba..991d174ee0 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -20,7 +20,9 @@ streamRatePosition (&_group, 4, 0, PSTR("POSITION")), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) + { + } void @@ -49,6 +51,8 @@ GCS_MAVLINK::update(void) { uint8_t c = comm_receive_ch(chan); + + // Try to get a new message if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } @@ -56,6 +60,11 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; + + + + + // send out queued params/ waypoints _queued_send(); @@ -84,7 +93,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working -// send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_NAV_CONTROLLER_OUTPUT); } if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { @@ -215,10 +224,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (mavlink_check_target(packet.target,packet.target_component)) break; + uint8_t result = 0; // do action - send_text_P(SEVERITY_LOW,PSTR("action received")); + send_text_P(SEVERITY_LOW,PSTR("action received: ")); +//Serial.println(packet.action); switch(packet.action){ case MAV_ACTION_LAUNCH: @@ -317,11 +328,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) default: break; } + mavlink_msg_action_ack_send( chan, packet.action, result ); + break; } @@ -332,7 +345,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; @@ -577,7 +589,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) 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 @@ -888,7 +899,6 @@ GCS_MAVLINK::_find_parameter(uint16_t index) void GCS_MAVLINK::_queued_send() { - // Check to see if we are sending parameters if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) { AP_Var *vp; @@ -915,9 +925,6 @@ GCS_MAVLINK::_queued_send() _queued_parameter_index++; } mavdelay = 0; - - - } // this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write @@ -941,4 +948,3 @@ GCS_MAVLINK::_queued_send() #endif - diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 07bb0e3067..b75d06a768 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -340,6 +340,8 @@ void Log_Write_Cmd(byte num, struct Location *wp) void Log_Write_Nav_Tuning() { + Matrix3f tempmat = dcm.get_dcm_matrix(); + DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_NAV_TUNING_MSG); @@ -359,35 +361,28 @@ void Log_Write_Nav_Tuning() DataFlash.WriteInt((int)next_WP.alt); // 11 DataFlash.WriteInt((int)altitude_error); // 12 + DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack + DataFlash.WriteLong(compass.last_update); // Just a temp hack + DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack + DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack + DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack + DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack + DataFlash.WriteByte(END_BYTE); } - -/* -Matrix3f tempmat = dcm.get_dcm_matrix(); - -DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); -DataFlash.WriteInt((int)wp_distance); -DataFlash.WriteInt((uint16_t)target_bearing); -DataFlash.WriteInt((uint16_t)nav_bearing); -DataFlash.WriteInt(altitude_error); -DataFlash.WriteInt((int)airspeed); -DataFlash.WriteInt((int)(nav_gain_scaler*1000)); -DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack -DataFlash.WriteLong(compass.last_update); // Just a temp hack -DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack -DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack -DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack -DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack -*/ - - // 1 2 3 4 5 6 7 8 9 10 11 //NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108 void Log_Read_Nav_Tuning() { // 1 2 3 4 5 6 7 8 9 10 11 - Serial.printf_P(PSTR("NTUN, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n"), + Serial.printf_P(PSTR( "NTUN, %d, %d, %d, " + "%d, %d, " + "%d, %d, %d, %d, " + "%d, %d, %d " + "%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f " + + ), DataFlash.ReadInt(), //yaw sensor DataFlash.ReadInt(), //distance DataFlash.ReadByte(), //bitmask @@ -402,11 +397,19 @@ void Log_Read_Nav_Tuning() DataFlash.ReadInt(), //home.alt DataFlash.ReadInt(), //Next_alt - DataFlash.ReadInt()); //Alt Error + DataFlash.ReadInt(), //Alt Error + + DataFlash.ReadInt(), + DataFlash.ReadLong(), + (float)DataFlash.ReadInt()/1000, + (float)DataFlash.ReadInt()/1000, + (float)DataFlash.ReadInt()/1000, + (float)DataFlash.ReadInt()/1000); } + // Write a mode packet. Total length : 5 bytes void Log_Write_Mode(byte mode) { @@ -492,7 +495,7 @@ void Log_Write_Current() DataFlash.WriteInt(g.rc_3.control_in); DataFlash.WriteLong(throttle_integrator); - DataFlash.WriteInt((int)(current_voltage * 100.0)); + DataFlash.WriteInt((int)(battery_voltage * 100.0)); DataFlash.WriteInt((int)(current_amps * 100.0)); DataFlash.WriteInt((int)current_total); @@ -772,4 +775,5 @@ void Log_Read(int start_page, int end_page) //Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count); } - + + diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 7ea696d364..8104befbb2 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -7,9 +7,11 @@ byte mavdelay = 0; + // what does this do? static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) { +//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC); if (sysid != mavlink_system.sysid){ return 1; @@ -22,6 +24,7 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } + void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) { uint64_t timeStamp = micros(); @@ -67,6 +70,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui } uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 10.0 * (float)(g.pack_capacity - current_total)/g.pack_capacity; //Mavlink scaling 100% = 1000 uint8_t motor_block = false; mavlink_msg_sys_status_send( @@ -75,7 +79,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui nav_mode, status, load * 1000, - battery_voltage1 * 1000, + battery_voltage * 1000, motor_block, packet_drops); break; @@ -110,6 +114,23 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui break; } + case MSG_NAV_CONTROLLER_OUTPUT: + { + //if (control_mode != MANUAL) { + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + 0, + crosstrack_error); + //} + break; + } + case MSG_LOCAL_LOCATION: { Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now @@ -277,4 +298,3 @@ void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8 #endif // mavlink in use #endif // inclusion guard - diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 17cc56a771..473bf74529 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -167,6 +167,7 @@ #define MSG_VERSION 0x09 #define MSG_EXTENDED_STATUS 0x0a #define MSG_CPU_LOAD 0x0b +#define MSG_NAV_CONTROLLER_OUTPUT 0x0c #define MSG_COMMAND_REQUEST 0x20 #define MSG_COMMAND_UPLOAD 0x21 @@ -297,5 +298,6 @@ // parameters get the first 1KiB of EEPROM, remainder is for waypoints #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_SIZE 15 + #define ONBOARD_PARAM_NAME_LENGTH 15 #define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 24fbe2c811..eca2fb08ab 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -155,7 +155,7 @@ void init_ardupilot() #else gcs.init(&Serial); #endif - + // init the HIL #if HIL_MODE != HIL_MODE_DISABLED @@ -469,10 +469,10 @@ void update_esc_light() void resetPerfData(void) { mainLoop_count = 0; G_Dt_max = 0; - gyro_sat_count = 0; - adc_constraints = 0; - renorm_sqrt_count = 0; - renorm_blowup_count = 0; + //gyro_sat_count = 0; + //adc_constraints = 0; + //renorm_sqrt_count = 0; + //renorm_blowup_count = 0; gps_fix_count = 0; perf_mon_timer = millis(); } @@ -525,4 +525,5 @@ init_throttle_cruise() //Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get()); } } -} +} + diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 00eb025548..e98d5dc401 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -654,7 +654,7 @@ test_current(uint8_t argc, const Menu::arg *argv) read_radio(); read_battery(); Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), - current_voltage, + battery_voltage, current_amps, current_total);