aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Mavlink Sync with APM

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2235 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-09 17:40:32 +00:00
parent 0979876ccc
commit a0d9c52d87
8 changed files with 84 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopterMega/ArduCopterMega.pde
View File

@ -43,9 +43,6 @@ version 2.1 of the License, or (at your option) any later version.
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <GCS_MAVLink.h> // 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
}
}

4
ArduCopterMega/GCS.h
View File

@ -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

24
ArduCopterMega/GCS_Mavlink.pde
View File

@ -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

52
ArduCopterMega/Log.pde
View File

@ -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);
}

24
ArduCopterMega/Mavlink_Common.h
View File

@ -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

2
ArduCopterMega/defines.h
View File

@ -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

13
ArduCopterMega/system.pde
View File

@ -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());
}
}
}
}

2
ArduCopterMega/test.pde
View File

@ -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);