Browse Source
this removes all the non-MAVLink GCS options, and simplifies the HIL and GCS code a lot. It also adds async sending of low priority GCS text messages.mission-4.1.18
Andrew Tridgell
14 years ago
26 changed files with 741 additions and 2029 deletions
@ -1,14 +0,0 @@
@@ -1,14 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
|
||||
#define HIL_PROTOCOL HIL_PROTOCOL_XPLANE |
||||
#define HIL_MODE HIL_MODE_ATTITUDE |
||||
#define HIL_PORT 0 |
||||
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK |
||||
#define GCS_PORT 3 |
||||
|
||||
#define AIRSPEED_CRUISE 25 |
||||
|
||||
#define THROTTLE_FAILSAFE ENABLED |
||||
#define AIRSPEED_SENSOR ENABLED |
||||
@ -1,147 +0,0 @@
@@ -1,147 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_LEGACY |
||||
|
||||
#if GCS_PORT == 3 |
||||
# define SendSer Serial3.print |
||||
# define SendSerln Serial3.println |
||||
#else |
||||
# define SendSer Serial.print |
||||
# define SendSerln Serial.println |
||||
#endif |
||||
|
||||
// This is the standard GCS output file for ArduPilot |
||||
|
||||
/* |
||||
Message Prefixes |
||||
!!! Position Low rate telemetry |
||||
+++ Attitude High rate telemetry |
||||
### Mode Change in control mode |
||||
%%% Waypoint Current and previous waypoints |
||||
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function |
||||
PPP IMU Performance Sent every 20 seconds for performance monitoring |
||||
|
||||
Message Suffix |
||||
*** All messages use this suffix |
||||
*/ |
||||
|
||||
/* |
||||
void static acknowledge(byte id, byte check1, byte check2) {} |
||||
void static send_message(byte id) {} |
||||
void static send_message(byte id, long param) {} |
||||
void static send_message(byte severity, const char *str) {} |
||||
*/ |
||||
|
||||
static void acknowledge(byte id, byte check1, byte check2) |
||||
{ |
||||
} |
||||
|
||||
static void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 |
||||
{ |
||||
if(severity >= DEBUG_LEVEL){ |
||||
SendSer("MSG: "); |
||||
SendSerln(str); |
||||
} |
||||
} |
||||
|
||||
static void send_message(byte id) |
||||
{ |
||||
send_message(id,0l); |
||||
} |
||||
|
||||
static void send_message(byte id, long param) |
||||
{ |
||||
switch(id) { |
||||
case MSG_ATTITUDE: // ** Attitude message |
||||
print_attitude(); |
||||
break; |
||||
case MSG_LOCATION: // ** Location/GPS message |
||||
print_position(); |
||||
break; |
||||
case MSG_HEARTBEAT: // ** Location/GPS message |
||||
print_control_mode(); |
||||
break; |
||||
//case MSG_PERF_REPORT: |
||||
// printPerfData(); |
||||
} |
||||
} |
||||
|
||||
static void print_current_waypoints() |
||||
{ |
||||
} |
||||
|
||||
static void print_attitude(void) |
||||
{ |
||||
SendSer("+++"); |
||||
SendSer("ASP:"); |
||||
SendSer((int)airspeed / 100, DEC); |
||||
SendSer(",THH:"); |
||||
SendSer(g.rc_3.servo_out, DEC); |
||||
SendSer (",RLL:"); |
||||
SendSer(dcm.roll_sensor / 100, DEC); |
||||
SendSer (",PCH:"); |
||||
SendSer(dcm.pitch_sensor / 100, DEC); |
||||
SendSerln(",***"); |
||||
} |
||||
|
||||
static void print_control_mode(void) |
||||
{ |
||||
SendSer("###"); |
||||
SendSer(flight_mode_strings[control_mode]); |
||||
SendSerln("***"); |
||||
} |
||||
|
||||
static void print_position(void) |
||||
{ |
||||
SendSer("!!"); |
||||
SendSer("!"); |
||||
SendSer("LAT:"); |
||||
SendSer(current_loc.lat/10,DEC); |
||||
SendSer(",LON:"); |
||||
SendSer(current_loc.lng/10,DEC); //wp_current_lat |
||||
SendSer(",SPD:"); |
||||
SendSer(g_gps->ground_speed/100,DEC); |
||||
SendSer(",CRT:"); |
||||
SendSer(climb_rate,DEC); |
||||
SendSer(",ALT:"); |
||||
SendSer(current_loc.alt/100,DEC); |
||||
SendSer(",ALH:"); |
||||
SendSer(next_WP.alt/100,DEC); |
||||
SendSer(",CRS:"); |
||||
SendSer(dcm.yaw_sensor/100,DEC); |
||||
SendSer(",BER:"); |
||||
SendSer(target_bearing/100,DEC); |
||||
SendSer(",WPN:"); |
||||
SendSer(g.waypoint_index,DEC);//Actually is the waypoint. |
||||
SendSer(",DST:"); |
||||
SendSer(wp_distance,DEC); |
||||
SendSer(",BTV:"); |
||||
SendSer(battery_voltage,DEC); |
||||
SendSer(",RSP:"); |
||||
SendSer(g.rc_1.servo_out/100,DEC); |
||||
SendSer(",TOW:"); |
||||
SendSer(g_gps->time); |
||||
SendSerln(",***"); |
||||
} |
||||
|
||||
static void print_waypoint(struct Location *cmd,byte index) |
||||
{ |
||||
SendSer("command #: "); |
||||
SendSer(index, DEC); |
||||
SendSer(" id: "); |
||||
SendSer(cmd->id,DEC); |
||||
SendSer(" p1: "); |
||||
SendSer(cmd->p1,DEC); |
||||
SendSer(" p2: "); |
||||
SendSer(cmd->alt,DEC); |
||||
SendSer(" p3: "); |
||||
SendSer(cmd->lat,DEC); |
||||
SendSer(" p4: "); |
||||
SendSerln(cmd->lng,DEC); |
||||
} |
||||
|
||||
static void print_waypoints() |
||||
{ |
||||
} |
||||
|
||||
#endif |
||||
@ -1,192 +0,0 @@
@@ -1,192 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_IMU |
||||
|
||||
// This is the standard GCS output file for ArduPilot |
||||
|
||||
/* |
||||
Message Prefixes |
||||
!!! Position Low rate telemetry |
||||
+++ Attitude High rate telemetry |
||||
### Mode Change in control mode |
||||
%%% Waypoint Current and previous waypoints |
||||
XXX Alert Text alert - NOTE: Alert message generation is not localized to a function |
||||
PPP IMU Performance Sent every 20 seconds for performance monitoring |
||||
|
||||
Message Suffix |
||||
*** All messages use this suffix |
||||
*/ |
||||
|
||||
/* |
||||
void acknowledge(byte id, byte check1, byte check2) {} |
||||
void send_message(byte id) {} |
||||
void send_message(byte id, long param) {} |
||||
void send_message(byte severity, const char *str) {} |
||||
*/ |
||||
|
||||
static void acknowledge(byte id, byte check1, byte check2) |
||||
{ |
||||
} |
||||
|
||||
static void send_message(byte id) |
||||
{ |
||||
send_message(id,0l); |
||||
} |
||||
|
||||
static void send_message(byte id, long param) |
||||
{ |
||||
switch(id) { |
||||
case MSG_ATTITUDE: |
||||
print_attitude(); |
||||
break; |
||||
|
||||
case MSG_LOCATION: // ** Location/GPS message |
||||
print_location(); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
static void send_message(byte severity, const char *str) |
||||
{ |
||||
if(severity >= DEBUG_LEVEL){ |
||||
Serial.print("MSG: "); |
||||
Serial.println(str); |
||||
} |
||||
} |
||||
|
||||
static void print_current_waypoints() |
||||
{ |
||||
} |
||||
|
||||
static void print_control_mode(void) |
||||
{ |
||||
} |
||||
|
||||
static void print_attitude(void) |
||||
{ |
||||
//Serial.print("!!VER:"); |
||||
//Serial.print(SOFTWARE_VER); //output the software version |
||||
//Serial.print(","); |
||||
|
||||
// Analogs |
||||
Serial.print("AN0:"); |
||||
Serial.print(read_adc(0)); //Reversing the sign. |
||||
Serial.print(",AN1:"); |
||||
Serial.print(read_adc(1)); |
||||
Serial.print(",AN2:"); |
||||
Serial.print(read_adc(2)); |
||||
Serial.print(",AN3:"); |
||||
Serial.print(read_adc(3)); |
||||
Serial.print (",AN4:"); |
||||
Serial.print(read_adc(4)); |
||||
Serial.print (",AN5:"); |
||||
Serial.print(read_adc(5)); |
||||
Serial.print (","); |
||||
|
||||
// DCM |
||||
Serial.print ("EX0:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[0][0])); |
||||
Serial.print (",EX1:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[0][1])); |
||||
Serial.print (",EX2:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[0][2])); |
||||
Serial.print (",EX3:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[1][0])); |
||||
Serial.print (",EX4:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[1][1])); |
||||
Serial.print (",EX5:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[1][2])); |
||||
Serial.print (",EX6:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[2][0])); |
||||
Serial.print (",EX7:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[2][1])); |
||||
Serial.print (",EX8:"); |
||||
Serial.print(convert_to_dec(DCM_Matrix[2][2])); |
||||
Serial.print (","); |
||||
|
||||
// Euler |
||||
Serial.print("RLL:"); |
||||
Serial.print(ToDeg(roll)); |
||||
Serial.print(",PCH:"); |
||||
Serial.print(ToDeg(pitch)); |
||||
Serial.print(",YAW:"); |
||||
Serial.print(ToDeg(yaw)); |
||||
Serial.print(",IMUH:"); |
||||
Serial.print(((int)imu_health>>8)&0xff); |
||||
Serial.print (","); |
||||
|
||||
|
||||
/* |
||||
#if USE_MAGNETOMETER == 1 |
||||
Serial.print("MGX:"); |
||||
Serial.print(magnetom_x); |
||||
Serial.print (",MGY:"); |
||||
Serial.print(magnetom_y); |
||||
Serial.print (",MGZ:"); |
||||
Serial.print(magnetom_z); |
||||
Serial.print (",MGH:"); |
||||
Serial.print(MAG_Heading); |
||||
Serial.print (","); |
||||
#endif |
||||
*/ |
||||
|
||||
//Serial.print("Temp:"); |
||||
//Serial.print(temp_unfilt/20.0); // Convert into degrees C |
||||
//alti(); |
||||
//Serial.print(",Pressure: "); |
||||
//Serial.print(press); |
||||
//Serial.print(",Alt: "); |
||||
//Serial.print(pressure_altitude/1000); // Original floating point full solution in meters |
||||
//Serial.print (","); |
||||
Serial.println("***"); |
||||
} |
||||
|
||||
void print_location(void) |
||||
{ |
||||
Serial.print("LAT:"); |
||||
Serial.print(current_loc.lat); |
||||
Serial.print(",LON:"); |
||||
Serial.print(current_loc.lng); |
||||
Serial.print(",ALT:"); |
||||
Serial.print(current_loc.alt/100); // meters |
||||
Serial.print(",COG:"); |
||||
Serial.print(g_gps->ground_course); |
||||
Serial.print(",SOG:"); |
||||
Serial.print(g_gps->ground_speed); |
||||
Serial.print(",FIX:"); |
||||
Serial.print((int)g_gps->fix); |
||||
Serial.print(",SAT:"); |
||||
Serial.print((int)g_gps->num_sats); |
||||
Serial.print (","); |
||||
Serial.print("TOW:"); |
||||
Serial.print(g_gps->time); |
||||
Serial.println("***"); |
||||
} |
||||
|
||||
static void print_waypoints() |
||||
{ |
||||
} |
||||
|
||||
static void print_waypoint(struct Location *cmd,byte index) |
||||
{ |
||||
Serial.print("command #: "); |
||||
Serial.print(index, DEC); |
||||
Serial.print(" id: "); |
||||
Serial.print(cmd->id,DEC); |
||||
Serial.print(" p1: "); |
||||
Serial.print(cmd->p1,DEC); |
||||
Serial.print(" p2: "); |
||||
Serial.print(cmd->alt,DEC); |
||||
Serial.print(" p3: "); |
||||
Serial.print(cmd->lat,DEC); |
||||
Serial.print(" p4: "); |
||||
Serial.println(cmd->lng,DEC); |
||||
|
||||
} |
||||
|
||||
static long convert_to_dec(float x) |
||||
{ |
||||
return x*10000000; |
||||
} |
||||
|
||||
#endif |
||||
@ -1,371 +0,0 @@
@@ -1,371 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
// Doug _ command index is a byte and not an int |
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_STANDARD |
||||
|
||||
static byte gcs_messages_sent; |
||||
|
||||
#if GCS_PORT == 3 |
||||
# define SendSer Serial3.print |
||||
#else |
||||
# define SendSer Serial.print |
||||
#endif |
||||
|
||||
// The functions included in this file are for use with the standard binary communication protocol and standard Ground Control Station |
||||
|
||||
static void acknowledge(byte id, byte check1, byte check2) { |
||||
byte mess_buffer[6]; |
||||
byte mess_ck_a = 0; |
||||
byte mess_ck_b = 0; |
||||
int ck; |
||||
|
||||
SendSer("4D"); // This is the message preamble |
||||
mess_buffer[0] = 0x03; |
||||
ck = 3; |
||||
mess_buffer[1] = 0x00; // Message ID |
||||
mess_buffer[2] = 0x01; // Message version |
||||
|
||||
mess_buffer[3] = id; |
||||
mess_buffer[4] = check1; |
||||
mess_buffer[5] = check2; |
||||
|
||||
|
||||
for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); |
||||
|
||||
for (int i = 0; i < ck + 3; i++) { |
||||
mess_ck_a += mess_buffer[i]; // Calculates checksums |
||||
mess_ck_b += mess_ck_a; |
||||
} |
||||
SendSer(mess_ck_a); |
||||
SendSer(mess_ck_b); |
||||
} |
||||
|
||||
static void send_message(byte id) { |
||||
send_message(id, 0l); |
||||
} |
||||
|
||||
static void send_message(byte id, long param) { |
||||
byte mess_buffer[54]; |
||||
byte mess_ck_a = 0; |
||||
byte mess_ck_b = 0; |
||||
int tempint; |
||||
int ck; |
||||
long templong; |
||||
|
||||
SendSer("4D"); // This is the message preamble |
||||
|
||||
switch(id) { |
||||
case MSG_HEARTBEAT: // ** System Status message |
||||
mess_buffer[0] = 0x07; |
||||
ck = 7; |
||||
mess_buffer[3] = control_mode; // Mode |
||||
templong = millis() / 1000; // Timestamp - Seconds since power - up |
||||
mess_buffer[4] = templong & 0xff; |
||||
mess_buffer[5] = (templong >> 8) & 0xff; |
||||
tempint = battery_voltage1 * 100; // Battery voltage ( * 100) |
||||
mess_buffer[6] = tempint & 0xff; |
||||
mess_buffer[7] = (tempint >> 8) & 0xff; |
||||
tempint = command_must_index; // Command Index (waypoint level) |
||||
mess_buffer[8] = tempint & 0xff; |
||||
mess_buffer[9] = (tempint >> 8) & 0xff; |
||||
break; |
||||
|
||||
case MSG_ATTITUDE: // ** Attitude message |
||||
mess_buffer[0] = 0x06; |
||||
ck = 6; |
||||
tempint = dcm.roll_sensor; // Roll (degrees * 100) |
||||
mess_buffer[3] = tempint & 0xff; |
||||
mess_buffer[4] = (tempint >> 8) & 0xff; |
||||
tempint = dcm.pitch_sensor; // Pitch (degrees * 100) |
||||
mess_buffer[5] = tempint & 0xff; |
||||
mess_buffer[6] = (tempint >> 8) & 0xff; |
||||
tempint = dcm.yaw_sensor; // Yaw (degrees * 100) |
||||
mess_buffer[7] = tempint & 0xff; |
||||
mess_buffer[8] = (tempint >> 8) & 0xff; |
||||
break; |
||||
|
||||
case MSG_LOCATION: // ** Location / GPS message |
||||
mess_buffer[0] = 0x12; |
||||
ck = 18; |
||||
templong = current_loc.lat; // Latitude *10 * *7 in 4 bytes |
||||
mess_buffer[3] = templong & 0xff; |
||||
mess_buffer[4] = (templong >> 8) & 0xff; |
||||
mess_buffer[5] = (templong >> 16) & 0xff; |
||||
mess_buffer[6] = (templong >> 24) & 0xff; |
||||
|
||||
templong = current_loc.lng; // Longitude *10 * *7 in 4 bytes |
||||
mess_buffer[7] = templong & 0xff; |
||||
mess_buffer[8] = (templong >> 8) & 0xff; |
||||
mess_buffer[9] = (templong >> 16) & 0xff; |
||||
mess_buffer[10] = (templong >> 24) & 0xff; |
||||
|
||||
tempint = g_gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes |
||||
mess_buffer[11] = tempint & 0xff; |
||||
mess_buffer[12] = (tempint >> 8) & 0xff; |
||||
|
||||
tempint = g_gps->ground_speed; // Speed in M / S * 100 in 2 bytes |
||||
mess_buffer[13] = tempint & 0xff; |
||||
mess_buffer[14] = (tempint >> 8) & 0xff; |
||||
|
||||
tempint = dcm.yaw_sensor; // Ground Course in degreees * 100 in 2 bytes |
||||
mess_buffer[15] = tempint & 0xff; |
||||
mess_buffer[16] = (tempint >> 8) & 0xff; |
||||
|
||||
templong = g_gps->time; // Time of Week (milliseconds) in 4 bytes |
||||
mess_buffer[17] = templong & 0xff; |
||||
mess_buffer[18] = (templong >> 8) & 0xff; |
||||
mess_buffer[19] = (templong >> 16) & 0xff; |
||||
mess_buffer[20] = (templong >> 24) & 0xff; |
||||
break; |
||||
|
||||
case MSG_PRESSURE: // ** Pressure message |
||||
mess_buffer[0] = 0x04; |
||||
ck = 4; |
||||
tempint = current_loc.alt / 10; // Altitude MSL in meters * 10 in 2 bytes |
||||
mess_buffer[3] = tempint & 0xff; |
||||
mess_buffer[4] = (tempint >> 8) & 0xff; |
||||
tempint = (int)airspeed / 100; // Airspeed pressure |
||||
mess_buffer[5] = tempint & 0xff; |
||||
mess_buffer[6] = (tempint >> 8) & 0xff; |
||||
break; |
||||
|
||||
// case 0xMSG_STATUS_TEXT: // ** Status Text message |
||||
// mess_buffer[0]=sizeof(status_message[0])+1; |
||||
// ck=mess_buffer[0]; |
||||
// mess_buffer[2] = param&0xff; |
||||
// for (int i=3;i<ck+2;i++) mess_buffer[i] = status_message[i-3]; |
||||
// break; |
||||
|
||||
case MSG_PERF_REPORT: // ** Performance Monitoring message |
||||
mess_buffer[0] = 0x10; |
||||
ck = 16; |
||||
templong = millis() - perf_mon_timer; // Report interval (milliseconds) in 4 bytes |
||||
mess_buffer[3] = templong & 0xff; |
||||
mess_buffer[4] = (templong >> 8) & 0xff; |
||||
mess_buffer[5] = (templong >> 16) & 0xff; |
||||
mess_buffer[6] = (templong >> 24) & 0xff; |
||||
tempint = mainLoop_count; // Main Loop cycles |
||||
mess_buffer[7] = tempint & 0xff; |
||||
mess_buffer[8] = (tempint >> 8) & 0xff; |
||||
mess_buffer[9] = 0 & 0xff; |
||||
mess_buffer[10] = gyro_sat_count; // Problem counts |
||||
mess_buffer[11] = adc_constraints; |
||||
mess_buffer[12] = renorm_sqrt_count; |
||||
mess_buffer[13] = renorm_blowup_count; |
||||
mess_buffer[14] = gps_fix_count; |
||||
tempint = (int)(imu_health * 1000); // IMU health metric |
||||
mess_buffer[15] = tempint & 0xff; |
||||
mess_buffer[16] = (tempint >> 8) & 0xff; |
||||
tempint = gcs_messages_sent; // GCS messages sent |
||||
mess_buffer[17] = tempint & 0xff; |
||||
mess_buffer[18] = (tempint >> 8) & 0xff; |
||||
break; |
||||
|
||||
case MSG_VALUE: // ** Requested Value message |
||||
mess_buffer[0] = 0x06; |
||||
ck = 6; |
||||
templong = param; // Parameter being sent |
||||
mess_buffer[3] = templong & 0xff; |
||||
mess_buffer[4] = (templong >> 8) & 0xff; |
||||
switch(param) { |
||||
/* |
||||
case 0x10: templong = integrator[0] * 1000; break; |
||||
case 0x11: templong = integrator[1] * 1000; break; |
||||
case 0x12: templong = integrator[2] * 1000; break; |
||||
case 0x13: templong = integrator[3] * 1000; break; |
||||
case 0x14: templong = integrator[4] * 1000; break; |
||||
case 0x15: templong = integrator[5] * 1000; break; |
||||
case 0x16: templong = integrator[6] * 1000; break; |
||||
case 0x17: templong = integrator[7] * 1000; break; |
||||
case 0x20: templong = target_bearing; break; |
||||
case 0x21: templong = nav_bearing; break; |
||||
case 0x22: templong = bearing_error; break; |
||||
case 0x23: templong = crosstrack_bearing; break; |
||||
case 0x24: templong = crosstrack_correction; break; |
||||
case 0x25: templong = altitude_error; break; |
||||
case 0x26: templong = wp_radius; break; |
||||
case 0x27: templong = loiter_radius; break; |
||||
// case 0x28: templong = wp_mode; break; |
||||
// case 0x29: templong = loop_commands; break; |
||||
*/ |
||||
} |
||||
mess_buffer[5] = templong & 0xff; |
||||
mess_buffer[6] = (templong >> 8) & 0xff; |
||||
mess_buffer[7] = (templong >> 16) & 0xff; |
||||
mess_buffer[8] = (templong >> 24) & 0xff; |
||||
break; |
||||
|
||||
case MSG_COMMAND: // Command list item message |
||||
mess_buffer[0] = 0x10; |
||||
ck = 16; |
||||
tempint = param; // item number |
||||
mess_buffer[3] = tempint & 0xff; |
||||
mess_buffer[4] = (tempint >> 8) & 0xff; |
||||
tempint = g.waypoint_total; // list length (# of commands in mission) |
||||
mess_buffer[5] = tempint & 0xff; |
||||
mess_buffer[6] = (tempint >> 8) & 0xff; |
||||
tell_command = get_command_with_index((int)param); |
||||
mess_buffer[7] = tell_command.id; // command id |
||||
mess_buffer[8] = tell_command.p1; // P1 |
||||
tempint = tell_command.alt; // P2 |
||||
mess_buffer[9] = tempint & 0xff; |
||||
mess_buffer[10] = (tempint >> 8) & 0xff; |
||||
templong = tell_command.lat; // P3 |
||||
mess_buffer[11] = templong & 0xff; |
||||
mess_buffer[12] = (templong >> 8) & 0xff; |
||||
mess_buffer[13] = (templong >> 16) & 0xff; |
||||
mess_buffer[14] = (templong >> 24) & 0xff; |
||||
templong = tell_command.lng; // P4 |
||||
mess_buffer[15] = templong & 0xff; |
||||
mess_buffer[16] = (templong >> 8) & 0xff; |
||||
mess_buffer[17] = (templong >> 16) & 0xff; |
||||
mess_buffer[18] = (templong >> 24) & 0xff; |
||||
break; |
||||
|
||||
case MSG_TRIMS: // Radio Trims message |
||||
//mess_buffer[0] = 0x10; |
||||
//ck = 16; |
||||
//for(int i = 0; i < 8; i++) { |
||||
// tempint = radio_trim[i]; // trim values |
||||
// mess_buffer[3 + 2 * i] = tempint & 0xff; |
||||
// mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; |
||||
//} |
||||
break; |
||||
|
||||
case MSG_MINS: // Radio Mins message |
||||
/*mess_buffer[0] = 0x10; |
||||
ck = 16; |
||||
for(int i = 0; i < 8; i++) { |
||||
tempint = radio_min[i]; // min values |
||||
mess_buffer[3 + 2 * i] = tempint & 0xff; |
||||
mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; |
||||
}*/ |
||||
break; |
||||
|
||||
case MSG_MAXS: // Radio Maxs message |
||||
/*mess_buffer[0] = 0x10; |
||||
ck = 16; |
||||
for(int i = 0; i < 8; i++) { |
||||
tempint = radio_max[i]; // max values |
||||
mess_buffer[3 + 2 * i] = tempint & 0xff; |
||||
mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; |
||||
}*/ |
||||
break; |
||||
|
||||
case MSG_PID: // PID Gains message |
||||
/* |
||||
mess_buffer[0] = 0x0f; |
||||
ck = 15; |
||||
mess_buffer[3] = param & 0xff; // PID set # |
||||
templong = (kp[param] * 1000000); // P gain |
||||
mess_buffer[4] = templong & 0xff; |
||||
mess_buffer[5] = (templong >> 8) & 0xff; |
||||
mess_buffer[6] = (templong >> 16) & 0xff; |
||||
mess_buffer[7] = (templong >> 24) & 0xff; |
||||
templong = (ki[param] * 1000000); // I gain |
||||
mess_buffer[8] = templong & 0xff; |
||||
mess_buffer[9] = (templong >> 8) & 0xff; |
||||
mess_buffer[10] = (templong >> 16) & 0xff; |
||||
mess_buffer[11] = (templong >> 24) & 0xff; |
||||
templong = (kd[param] * 1000000); // D gain |
||||
mess_buffer[12] = templong & 0xff; |
||||
mess_buffer[13] = (templong >> 8) & 0xff; |
||||
mess_buffer[14] = (templong >> 16) & 0xff; |
||||
mess_buffer[15] = (templong >> 24) & 0xff; |
||||
tempint = integrator_max[param]; // Integrator max value |
||||
mess_buffer[16] = tempint & 0xff; |
||||
mess_buffer[17] = (tempint >> 8) & 0xff; |
||||
*/ |
||||
break; |
||||
} |
||||
|
||||
//mess_buffer[0] = length; // Message length |
||||
mess_buffer[1] = id; // Message ID |
||||
mess_buffer[2] = 0x01; // Message version |
||||
|
||||
for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); |
||||
|
||||
for (int i = 0; i < ck + 3; i++) { |
||||
mess_ck_a += mess_buffer[i]; // Calculates checksums |
||||
mess_ck_b += mess_ck_a; |
||||
} |
||||
|
||||
SendSer(mess_ck_a); |
||||
SendSer(mess_ck_b); |
||||
|
||||
gcs_messages_sent++; |
||||
} |
||||
|
||||
static void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT |
||||
{ |
||||
if(severity >= DEBUG_LEVEL){ |
||||
byte length = strlen(str) + 1; |
||||
|
||||
byte mess_buffer[54]; |
||||
byte mess_ck_a = 0; |
||||
byte mess_ck_b = 0; |
||||
int ck; |
||||
|
||||
SendSer("4D"); // This is the message preamble |
||||
if(length > 50) length = 50; |
||||
mess_buffer[0] = length; |
||||
ck = length; |
||||
mess_buffer[1] = 0x05; // Message ID |
||||
mess_buffer[2] = 0x01; // Message version |
||||
|
||||
mess_buffer[3] = severity; |
||||
|
||||
for (int i = 3; i < ck + 2; i++) |
||||
mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+ |
||||
|
||||
for (int i = 0; i < ck + 3; i++) |
||||
SendSer(mess_buffer[i]); |
||||
|
||||
for (int i = 0; i < ck + 3; i++) { |
||||
mess_ck_a += mess_buffer[i]; // Calculates checksums |
||||
mess_ck_b += mess_ck_a; |
||||
} |
||||
SendSer(mess_ck_a); |
||||
SendSer(mess_ck_b); |
||||
} |
||||
} |
||||
|
||||
static void print_current_waypoints() |
||||
{ |
||||
} |
||||
|
||||
static void print_waypoint(struct Location *cmd, byte index) |
||||
{ |
||||
Serial.print("command #: "); |
||||
Serial.print(index, DEC); |
||||
Serial.print(" id: "); |
||||
Serial.print(cmd->id, DEC); |
||||
Serial.print(" p1: "); |
||||
Serial.print(cmd->p1, DEC); |
||||
Serial.print(" p2: "); |
||||
Serial.print(cmd->alt, DEC); |
||||
Serial.print(" p3: "); |
||||
Serial.print(cmd->lat, DEC); |
||||
Serial.print(" p4: "); |
||||
Serial.println(cmd->lng, DEC); |
||||
} |
||||
|
||||
static void print_waypoints() |
||||
{ |
||||
} |
||||
|
||||
|
||||
#endif |
||||
|
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_NONE |
||||
static void acknowledge(byte id, byte check1, byte check2) {} |
||||
static void send_message(byte id) {} |
||||
static void send_message(byte id, long param) {} |
||||
static void send_message(byte severity, const char *str) { |
||||
Serial.println(str); |
||||
} |
||||
static void print_current_waypoints(){} |
||||
static void print_waypoint(struct Location *cmd, byte index){} |
||||
static void print_waypoints(){} |
||||
#endif |
||||
@ -1,127 +0,0 @@
@@ -1,127 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_XPLANE |
||||
|
||||
|
||||
void acknowledge(byte id, byte check1, byte check2) |
||||
{ |
||||
} |
||||
|
||||
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 |
||||
{ |
||||
if(severity >= DEBUG_LEVEL){ |
||||
Serial.print("MSG: "); |
||||
Serial.println(str); |
||||
} |
||||
} |
||||
|
||||
void send_message(byte id) { |
||||
send_message(id,0l); |
||||
} |
||||
|
||||
void send_message(byte id, long param) { |
||||
switch(id) { |
||||
case MSG_HEARTBEAT: |
||||
print_control_mode(); |
||||
break; |
||||
|
||||
case MSG_ATTITUDE: |
||||
//print_attitude(); |
||||
break; |
||||
|
||||
case MSG_LOCATION: |
||||
//print_position(); |
||||
break; |
||||
|
||||
case MSG_COMMAND: |
||||
struct Location cmd = get_command_with_index(param); |
||||
print_waypoint(&cmd, param); |
||||
break; |
||||
|
||||
} |
||||
} |
||||
|
||||
|
||||
// required by Groundstation to plot lateral tracking course |
||||
void print_new_wp_info() |
||||
{ |
||||
} |
||||
|
||||
void print_control_mode(void) |
||||
{ |
||||
Serial.print("MSG: "); |
||||
Serial.print(flight_mode_strings[control_mode]); |
||||
} |
||||
|
||||
|
||||
void print_current_waypoints() |
||||
{ |
||||
Serial.print("MSG: "); |
||||
Serial.print("CUR:"); |
||||
Serial.print("\t"); |
||||
Serial.print(current_loc.lat,DEC); |
||||
Serial.print(",\t"); |
||||
Serial.print(current_loc.lng,DEC); |
||||
Serial.print(",\t"); |
||||
Serial.println(current_loc.alt,DEC); |
||||
|
||||
Serial.print("MSG: "); |
||||
Serial.print("NWP:"); |
||||
Serial.print(g.waypoint_index,DEC); |
||||
Serial.print(",\t"); |
||||
Serial.print(next_WP.lat,DEC); |
||||
Serial.print(",\t"); |
||||
Serial.print(next_WP.lng,DEC); |
||||
Serial.print(",\t"); |
||||
Serial.println(next_WP.alt,DEC); |
||||
} |
||||
|
||||
void print_position(void) |
||||
{ |
||||
} |
||||
|
||||
void printPerfData(void) |
||||
{ |
||||
} |
||||
|
||||
void print_attitude(void) |
||||
{ |
||||
} |
||||
void print_tuning(void) { |
||||
} |
||||
void print_waypoint(struct Location *cmd,byte index) |
||||
{ |
||||
Serial.print("MSG: command #: "); |
||||
Serial.print(index, DEC); |
||||
Serial.print(" id: "); |
||||
Serial.print(cmd->id,DEC); |
||||
Serial.print(" p1: "); |
||||
Serial.print(cmd->p1,DEC); |
||||
Serial.print(" p2: "); |
||||
Serial.print(cmd->alt,DEC); |
||||
Serial.print(" p3: "); |
||||
Serial.print(cmd->lat,DEC); |
||||
Serial.print(" p4: "); |
||||
Serial.println(cmd->lng,DEC); |
||||
} |
||||
|
||||
void print_waypoints() |
||||
{ |
||||
Serial.println("Commands in memory"); |
||||
Serial.print("commands total: "); |
||||
Serial.println(g.waypoint_total, DEC); |
||||
// create a location struct to hold the temp Waypoints for printing |
||||
//Location tmp; |
||||
Serial.println("Home: "); |
||||
struct Location cmd = get_command_with_index(0); |
||||
print_waypoint(&cmd, 0); |
||||
Serial.println("Commands: "); |
||||
|
||||
for (int i=1; i < g.waypoint_total; i++){ |
||||
cmd = get_command_with_index(i); |
||||
print_waypoint(&cmd, i); |
||||
} |
||||
} |
||||
|
||||
#endif |
||||
|
||||
@ -1,135 +0,0 @@
@@ -1,135 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/// @file HIL.h
|
||||
/// @brief Interface definition for the various Ground Control System protocols.
|
||||
|
||||
#ifndef __HIL_H |
||||
#define __HIL_H |
||||
|
||||
# if HIL_MODE != HIL_MODE_DISABLED |
||||
|
||||
#include <BetterStream.h> |
||||
#include <AP_Common.h> |
||||
#include <GPS.h> |
||||
#include <Stream.h> |
||||
#include <stdint.h> |
||||
|
||||
///
|
||||
/// @class HIL
|
||||
/// @brief Class describing the interface between the APM code
|
||||
/// proper and the HIL implementation.
|
||||
///
|
||||
/// HIL' are currently implemented inside the sketch and as such have
|
||||
/// access to all global state. The sketch should not, however, call HIL
|
||||
/// internal functions - all calls to the HIL should be routed through
|
||||
/// this interface (or functions explicitly exposed by a subclass).
|
||||
///
|
||||
class HIL_Class |
||||
{ |
||||
public: |
||||
|
||||
/// Startup initialisation.
|
||||
///
|
||||
/// This routine performs any one-off initialisation required before
|
||||
/// HIL messages are exchanged.
|
||||
///
|
||||
/// @note The stream is expected to be set up and configured for the
|
||||
/// correct bitrate before ::init is called.
|
||||
///
|
||||
/// @note The stream is currently BetterStream so that we can use the _P
|
||||
/// methods; this may change if Arduino adds them to Print.
|
||||
///
|
||||
/// @param port The stream over which messages are exchanged.
|
||||
///
|
||||
void init(BetterStream *port) { _port = port; } |
||||
|
||||
/// Update HIL state.
|
||||
///
|
||||
/// This may involve checking for received bytes on the stream,
|
||||
/// or sending additional periodic messages.
|
||||
void update(void) {} |
||||
|
||||
/// Send a message with a single numeric parameter.
|
||||
///
|
||||
/// This may be a standalone message, or the HIL driver may
|
||||
/// have its own way of locating additional parameters to send.
|
||||
///
|
||||
/// @param id ID of the message to send.
|
||||
/// @param param Explicit message parameter.
|
||||
///
|
||||
void send_message(uint8_t id, int32_t param = 0) {} |
||||
|
||||
/// Send a text message.
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text(uint8_t severity, const char *str) {} |
||||
|
||||
/// Send a text message with a PSTR()
|
||||
///
|
||||
/// @param severity A value describing the importance of the message.
|
||||
/// @param str The text to be sent.
|
||||
///
|
||||
void send_text(uint8_t severity, const prog_char_t *str) {} |
||||
|
||||
/// Send acknowledgement for a message.
|
||||
///
|
||||
/// @param id The message ID being acknowledged.
|
||||
/// @param sum1 Checksum byte 1 from the message being acked.
|
||||
/// @param sum2 Checksum byte 2 from the message being acked.
|
||||
///
|
||||
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {} |
||||
|
||||
protected: |
||||
/// The stream we are communicating over
|
||||
BetterStream *_port; |
||||
}; |
||||
|
||||
//
|
||||
// HIL class definitions.
|
||||
//
|
||||
// These are here so that we can declare the HIL object early in the sketch
|
||||
// and then reference it statically rather than via a pointer.
|
||||
//
|
||||
|
||||
///
|
||||
/// @class HIL_MAVLINK
|
||||
/// @brief The mavlink protocol for hil
|
||||
///
|
||||
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK |
||||
// uses gcs
|
||||
#endif // HIL_PROTOCOL_MAVLINK
|
||||
|
||||
///
|
||||
/// @class HIL_XPLANE
|
||||
/// @brief The xplane protocol for hil
|
||||
///
|
||||
#if HIL_PROTOCOL == HIL_PROTOCOL_XPLANE |
||||
class HIL_XPLANE : public HIL_Class |
||||
{ |
||||
public: |
||||
HIL_XPLANE(); |
||||
void update(void); |
||||
void init(BetterStream *port); |
||||
void send_message(uint8_t id, uint32_t param = 0); |
||||
void send_text(uint8_t severity, const char *str); |
||||
void send_text(uint8_t severity, const prog_char_t *str); |
||||
void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); |
||||
private: |
||||
void output_HIL(); |
||||
void output_HIL_(); |
||||
void output_int(int value); |
||||
void output_byte(byte value); |
||||
void print_buffer(); |
||||
|
||||
AP_GPS_IMU * hilPacketDecoder; |
||||
byte buf_len; |
||||
byte out_buffer[32]; |
||||
}; |
||||
#endif // HIL_PROTOCOL_XPLANE
|
||||
|
||||
#endif // HIL not disabled
|
||||
|
||||
#endif // __HIL_H
|
||||
|
||||
@ -1,131 +0,0 @@
@@ -1,131 +0,0 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
||||
|
||||
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_XPLANE |
||||
|
||||
|
||||
void HIL_XPLANE::output_HIL(void) |
||||
{ |
||||
// output real-time sensor data |
||||
Serial.print("AAA"); // Message preamble |
||||
output_int((int)(g.rc_1.servo_out)); // 0 bytes 0, 1 |
||||
output_int((int)(g.rc_2.servo_out)); // 1 bytes 2, 3 |
||||
output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5 |
||||
output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7 |
||||
output_int((int)wp_distance); // 4 bytes 8,9 |
||||
//output_int((int)bearing_error); // 5 bytes 10,11 |
||||
output_int((int)altitude_error); // 6 bytes 12, 13 |
||||
output_int((int)energy_error); // 7 bytes 14,15 |
||||
output_byte((int)g.waypoint_index); // 8 bytes 16 |
||||
output_byte(control_mode); // 9 bytes 17 |
||||
|
||||
// print out the buffer and checksum |
||||
// --------------------------------- |
||||
print_buffer(); |
||||
} |
||||
|
||||
void HIL_XPLANE::output_int(int value) |
||||
{ |
||||
//buf_len += 2; |
||||
out_buffer[buf_len++] = value & 0xff; |
||||
out_buffer[buf_len++] = (value >> 8) & 0xff; |
||||
} |
||||
|
||||
void HIL_XPLANE::output_byte(byte value) |
||||
{ |
||||
out_buffer[buf_len++] = value; |
||||
} |
||||
|
||||
void HIL_XPLANE::print_buffer() |
||||
{ |
||||
byte ck_a = 0; |
||||
byte ck_b = 0; |
||||
for (byte i = 0;i < buf_len; i++){ |
||||
Serial.print (out_buffer[i]); |
||||
} |
||||
Serial.print('\r'); |
||||
Serial.print('\n'); |
||||
buf_len = 0; |
||||
} |
||||
|
||||
|
||||
|
||||
HIL_XPLANE::HIL_XPLANE() : |
||||
buf_len(0) |
||||
{ |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::init(BetterStream * port) |
||||
{ |
||||
HIL_Class::init(port); |
||||
hilPacketDecoder = new AP_GPS_IMU(port); |
||||
hilPacketDecoder->init(); |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::update(void) |
||||
{ |
||||
hilPacketDecoder->update(); |
||||
airspeed = (float)hilPacketDecoder->airspeed; //airspeed is * 100 coming in from Xplane for accuracy |
||||
calc_airspeed_errors(); |
||||
dcm.setHil(hilPacketDecoder->roll_sensor*M_PI/18000, |
||||
hilPacketDecoder->pitch_sensor*M_PI/18000, |
||||
hilPacketDecoder->ground_course*M_PI/18000, |
||||
0,0,0); |
||||
|
||||
// set gps hil sensor |
||||
g_gps->setHIL(hilPacketDecoder->time/1000.0,(float)hilPacketDecoder->latitude/1.0e7,(float)hilPacketDecoder->longitude/1.0e7,(float)hilPacketDecoder->altitude/1.0e2, |
||||
(float)hilPacketDecoder->speed_3d/1.0e2,(float)hilPacketDecoder->ground_course/1.0e2,0,0); |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::send_message(uint8_t id, uint32_t param) |
||||
{ |
||||
// TODO: split output by actual request |
||||
uint64_t timeStamp = micros(); |
||||
switch(id) { |
||||
|
||||
case MSG_HEARTBEAT: |
||||
break; |
||||
case MSG_EXTENDED_STATUS: |
||||
break; |
||||
case MSG_ATTITUDE: |
||||
break; |
||||
case MSG_LOCATION: |
||||
break; |
||||
case MSG_GPS_RAW: |
||||
break; |
||||
case MSG_SERVO_OUT: |
||||
output_HIL(); |
||||
break; |
||||
case MSG_RADIO_OUT: |
||||
break; |
||||
case MSG_RAW_IMU1: |
||||
case MSG_RAW_IMU2: |
||||
case MSG_RAW_IMU3: |
||||
break; |
||||
case MSG_GPS_STATUS: |
||||
break; |
||||
case MSG_CURRENT_WAYPOINT: |
||||
break; |
||||
defualt: |
||||
break; |
||||
} |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::send_text(uint8_t severity, const char *str) |
||||
{ |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::send_text(uint8_t severity, const prog_char_t *str) |
||||
{ |
||||
} |
||||
|
||||
void |
||||
HIL_XPLANE::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) |
||||
{ |
||||
} |
||||
|
||||
#endif |
||||
@ -1,456 +0,0 @@
@@ -1,456 +0,0 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#ifndef Mavlink_Common_H |
||||
#define Mavlink_Common_H |
||||
|
||||
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK |
||||
|
||||
byte mavdelay = 0; |
||||
|
||||
|
||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false |
||||
|
||||
/*
|
||||
!!NOTE!! |
||||
|
||||
the use of NOINLINE separate functions for each message type avoids |
||||
a compiler bug in gcc that would cause it to use far more stack |
||||
space than is needed. Without the NOINLINE we use the sum of the |
||||
stack needed for each message type. Please be careful to follow the |
||||
pattern below when adding any new messages |
||||
*/ |
||||
|
||||
#define NOINLINE __attribute__((noinline)) |
||||
|
||||
static NOINLINE void send_heartbeat(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_heartbeat_send( |
||||
chan, |
||||
mavlink_system.type, |
||||
MAV_AUTOPILOT_ARDUPILOTMEGA); |
||||
} |
||||
|
||||
static NOINLINE void send_attitude(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_attitude_send( |
||||
chan, |
||||
micros(), |
||||
dcm.roll, |
||||
dcm.pitch, |
||||
dcm.yaw, |
||||
omega.x, |
||||
omega.y, |
||||
omega.z); |
||||
} |
||||
|
||||
static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) |
||||
{ |
||||
uint8_t mode = MAV_MODE_UNINIT; |
||||
uint8_t nav_mode = MAV_NAV_VECTOR; |
||||
|
||||
switch(control_mode) { |
||||
case LOITER: |
||||
mode = MAV_MODE_AUTO; |
||||
nav_mode = MAV_NAV_HOLD; |
||||
break; |
||||
case AUTO: |
||||
mode = MAV_MODE_AUTO; |
||||
nav_mode = MAV_NAV_WAYPOINT; |
||||
break; |
||||
case RTL: |
||||
mode = MAV_MODE_AUTO; |
||||
nav_mode = MAV_NAV_RETURNING; |
||||
break; |
||||
case GUIDED: |
||||
mode = MAV_MODE_GUIDED; |
||||
break; |
||||
default: |
||||
mode = control_mode + 100; |
||||
} |
||||
|
||||
uint8_t status = MAV_STATE_ACTIVE; |
||||
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
|
||||
|
||||
mavlink_msg_sys_status_send( |
||||
chan, |
||||
mode, |
||||
nav_mode, |
||||
status, |
||||
0, |
||||
battery_voltage * 1000, |
||||
battery_remaining, |
||||
packet_drops); |
||||
} |
||||
|
||||
static void NOINLINE send_meminfo(mavlink_channel_t chan) |
||||
{ |
||||
extern unsigned __brkval; |
||||
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); |
||||
} |
||||
|
||||
static void NOINLINE send_location(mavlink_channel_t chan) |
||||
{ |
||||
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
|
||||
mavlink_msg_global_position_int_send( |
||||
chan, |
||||
current_loc.lat, |
||||
current_loc.lng, |
||||
current_loc.alt * 10, |
||||
g_gps->ground_speed * rot.a.x, |
||||
g_gps->ground_speed * rot.b.x, |
||||
g_gps->ground_speed * rot.c.x); |
||||
} |
||||
|
||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_nav_controller_output_send( |
||||
chan, |
||||
nav_roll / 1.0e2, |
||||
nav_pitch / 1.0e2, |
||||
target_bearing / 1.0e2, |
||||
target_bearing / 1.0e2, |
||||
wp_distance, |
||||
altitude_error / 1.0e2, |
||||
0, |
||||
0); |
||||
} |
||||
|
||||
static void NOINLINE send_gps_raw(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_gps_raw_send( |
||||
chan, |
||||
micros(), |
||||
g_gps->status(), |
||||
g_gps->latitude / 1.0e7, |
||||
g_gps->longitude / 1.0e7, |
||||
g_gps->altitude / 100.0, |
||||
g_gps->hdop, |
||||
0.0, |
||||
g_gps->ground_speed / 100.0, |
||||
g_gps->ground_course / 100.0); |
||||
} |
||||
|
||||
static void NOINLINE send_servo_out(mavlink_channel_t chan) |
||||
{ |
||||
const uint8_t rssi = 1; |
||||
// normalized values scaled to -10000 to 10000
|
||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME |
||||
|
||||
mavlink_msg_rc_channels_scaled_send( |
||||
chan, |
||||
g.rc_1.servo_out,
|
||||
g.rc_2.servo_out,
|
||||
g.rc_3.radio_out,
|
||||
g.rc_4.servo_out, |
||||
0, |
||||
0, |
||||
0, |
||||
0, |
||||
rssi); |
||||
|
||||
#else |
||||
|
||||
mavlink_msg_rc_channels_scaled_send( |
||||
chan, |
||||
g.rc_1.servo_out, |
||||
g.rc_2.servo_out, |
||||
g.rc_3.radio_out, |
||||
g.rc_4.servo_out, |
||||
10000 * g.rc_1.norm_output(), |
||||
10000 * g.rc_2.norm_output(), |
||||
10000 * g.rc_3.norm_output(), |
||||
10000 * g.rc_4.norm_output(), |
||||
rssi); |
||||
|
||||
#endif |
||||
} |
||||
|
||||
static void NOINLINE send_radio_in(mavlink_channel_t chan) |
||||
{ |
||||
const uint8_t rssi = 1; |
||||
mavlink_msg_rc_channels_raw_send( |
||||
chan, |
||||
g.rc_1.radio_in, |
||||
g.rc_2.radio_in, |
||||
g.rc_3.radio_in, |
||||
g.rc_4.radio_in, |
||||
g.rc_5.radio_in, |
||||
g.rc_6.radio_in, |
||||
g.rc_7.radio_in, |
||||
g.rc_8.radio_in, |
||||
rssi); |
||||
} |
||||
|
||||
static void NOINLINE send_radio_out(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_servo_output_raw_send( |
||||
chan, |
||||
motor_out[0], |
||||
motor_out[1], |
||||
motor_out[2], |
||||
motor_out[3], |
||||
motor_out[4], |
||||
motor_out[5], |
||||
motor_out[6], |
||||
motor_out[7]); |
||||
} |
||||
|
||||
static void NOINLINE send_vfr_hud(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_vfr_hud_send( |
||||
chan, |
||||
(float)airspeed / 100.0, |
||||
(float)g_gps->ground_speed / 100.0, |
||||
(dcm.yaw_sensor / 100) % 360, |
||||
g.rc_3.servo_out/10, |
||||
current_loc.alt / 100.0, |
||||
climb_rate); |
||||
} |
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE |
||||
static void NOINLINE send_raw_imu1(mavlink_channel_t chan) |
||||
{ |
||||
Vector3f accel = imu.get_accel(); |
||||
Vector3f gyro = imu.get_gyro(); |
||||
mavlink_msg_raw_imu_send( |
||||
chan, |
||||
micros(), |
||||
accel.x * 1000.0 / gravity, |
||||
accel.y * 1000.0 / gravity, |
||||
accel.z * 1000.0 / gravity, |
||||
gyro.x * 1000.0, |
||||
gyro.y * 1000.0, |
||||
gyro.z * 1000.0, |
||||
compass.mag_x, |
||||
compass.mag_y, |
||||
compass.mag_z); |
||||
} |
||||
|
||||
static void NOINLINE send_raw_imu2(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_scaled_pressure_send( |
||||
chan, |
||||
micros(), |
||||
(float)barometer.Press/100.0, |
||||
(float)(barometer.Press-ground_pressure)/100.0, |
||||
(int)(barometer.Temp*10)); |
||||
} |
||||
|
||||
static void NOINLINE send_raw_imu3(mavlink_channel_t chan) |
||||
{ |
||||
Vector3f mag_offsets = compass.get_offsets(); |
||||
|
||||
mavlink_msg_sensor_offsets_send(chan, |
||||
mag_offsets.x, |
||||
mag_offsets.y, |
||||
mag_offsets.z, |
||||
compass.get_declination(), |
||||
barometer.RawPress, |
||||
barometer.RawTemp, |
||||
imu.gx(), imu.gy(), imu.gz(), |
||||
imu.ax(), imu.ay(), imu.az()); |
||||
} |
||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
static void NOINLINE send_gps_status(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_gps_status_send( |
||||
chan, |
||||
g_gps->num_sats, |
||||
NULL, |
||||
NULL, |
||||
NULL, |
||||
NULL, |
||||
NULL); |
||||
} |
||||
|
||||
static void NOINLINE send_current_waypoint(mavlink_channel_t chan) |
||||
{ |
||||
mavlink_msg_waypoint_current_send( |
||||
chan, |
||||
g.waypoint_index); |
||||
} |
||||
|
||||
// try to send a message, return false if it won't fit in the serial tx buffer
|
||||
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) |
||||
{ |
||||
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; |
||||
|
||||
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { |
||||
// defer any messages on the telemetry port for 1 second after
|
||||
// bootup, to try to prevent bricking of Xbees
|
||||
return false; |
||||
} |
||||
|
||||
switch(id) { |
||||
case MSG_HEARTBEAT: |
||||
CHECK_PAYLOAD_SIZE(HEARTBEAT); |
||||
send_heartbeat(chan); |
||||
break; |
||||
|
||||
case MSG_EXTENDED_STATUS1: |
||||
CHECK_PAYLOAD_SIZE(SYS_STATUS); |
||||
send_extended_status1(chan, packet_drops); |
||||
break; |
||||
|
||||
case MSG_EXTENDED_STATUS2: |
||||
CHECK_PAYLOAD_SIZE(MEMINFO); |
||||
send_meminfo(chan); |
||||
break; |
||||
|
||||
case MSG_ATTITUDE: |
||||
CHECK_PAYLOAD_SIZE(ATTITUDE); |
||||
send_attitude(chan); |
||||
break; |
||||
|
||||
case MSG_LOCATION: |
||||
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); |
||||
send_location(chan); |
||||
break; |
||||
|
||||
case MSG_NAV_CONTROLLER_OUTPUT: |
||||
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); |
||||
send_nav_controller_output(chan); |
||||
break; |
||||
|
||||
case MSG_GPS_RAW: |
||||
CHECK_PAYLOAD_SIZE(GPS_RAW); |
||||
send_gps_raw(chan); |
||||
break; |
||||
|
||||
case MSG_SERVO_OUT: |
||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); |
||||
send_servo_out(chan); |
||||
break; |
||||
|
||||
case MSG_RADIO_IN: |
||||
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); |
||||
send_radio_in(chan); |
||||
break; |
||||
|
||||
case MSG_RADIO_OUT: |
||||
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); |
||||
send_radio_out(chan); |
||||
break; |
||||
|
||||
case MSG_VFR_HUD: |
||||
CHECK_PAYLOAD_SIZE(VFR_HUD); |
||||
send_vfr_hud(chan); |
||||
break; |
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE |
||||
case MSG_RAW_IMU1: |
||||
CHECK_PAYLOAD_SIZE(RAW_IMU); |
||||
send_raw_imu1(chan); |
||||
break; |
||||
|
||||
case MSG_RAW_IMU2: |
||||
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); |
||||
send_raw_imu2(chan); |
||||
break; |
||||
|
||||
case MSG_RAW_IMU3: |
||||
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); |
||||
send_raw_imu3(chan); |
||||
break; |
||||
#endif // HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
case MSG_GPS_STATUS: |
||||
CHECK_PAYLOAD_SIZE(GPS_STATUS); |
||||
send_gps_status(chan); |
||||
break; |
||||
|
||||
case MSG_CURRENT_WAYPOINT: |
||||
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); |
||||
send_current_waypoint(chan); |
||||
break; |
||||
|
||||
default: |
||||
break; |
||||
} |
||||
return true; |
||||
} |
||||
|
||||
|
||||
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
|
||||
// switch types in mavlink_try_send_message()
|
||||
static struct mavlink_queue { |
||||
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; |
||||
uint8_t next_deferred_message; |
||||
uint8_t num_deferred_messages; |
||||
} mavlink_queue[2]; |
||||
|
||||
// send a message using mavlink
|
||||
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops) |
||||
{ |
||||
uint8_t i, nextid; |
||||
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan]; |
||||
|
||||
// see if we can send the deferred messages, if any
|
||||
while (q->num_deferred_messages != 0) { |
||||
if (!mavlink_try_send_message(chan, |
||||
q->deferred_messages[q->next_deferred_message], |
||||
packet_drops)) { |
||||
break; |
||||
} |
||||
q->next_deferred_message++; |
||||
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) { |
||||
q->next_deferred_message = 0; |
||||
} |
||||
q->num_deferred_messages--; |
||||
} |
||||
|
||||
if (id == MSG_RETRY_DEFERRED) { |
||||
return; |
||||
} |
||||
|
||||
// this message id might already be deferred
|
||||
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) { |
||||
if (q->deferred_messages[nextid] == id) { |
||||
// its already deferred, discard
|
||||
return; |
||||
} |
||||
nextid++; |
||||
if (nextid == MAX_DEFERRED_MESSAGES) { |
||||
nextid = 0; |
||||
} |
||||
} |
||||
|
||||
if (q->num_deferred_messages != 0 || |
||||
!mavlink_try_send_message(chan, id, packet_drops)) { |
||||
// can't send it now, so defer it
|
||||
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) { |
||||
// the defer buffer is full, discard
|
||||
return; |
||||
} |
||||
nextid = q->next_deferred_message + q->num_deferred_messages; |
||||
if (nextid >= MAX_DEFERRED_MESSAGES) { |
||||
nextid -= MAX_DEFERRED_MESSAGES; |
||||
} |
||||
q->deferred_messages[nextid] = id; |
||||
q->num_deferred_messages++; |
||||
} |
||||
} |
||||
|
||||
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) |
||||
{ |
||||
if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { |
||||
// don't send status MAVLink messages for 1 second after
|
||||
// bootup, to try to prevent Xbee bricking
|
||||
return; |
||||
} |
||||
mavlink_msg_statustext_send( |
||||
chan, |
||||
severity, |
||||
(const int8_t*) str); |
||||
} |
||||
|
||||
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2) |
||||
{ |
||||
} |
||||
|
||||
#endif // mavlink in use
|
||||
|
||||
#endif // inclusion guard
|
||||
Loading…
Reference in new issue