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

mavlink: fixed build with MAVLink and HIL

Browse Source 
this fixes the build with HIL_MODE_ATTITUDE and MAVLink enabled. Basic
MAVLink operation works.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1737 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-03-03 11:39:52 +00:00
parent 988149a1c9
commit 7691133230
8 changed files with 83 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopterMega/ArduCopterMega.pde
View File

@ -360,7 +360,7 @@ struct Location home; // home location
struct Location prev_WP; // last waypoint struct Location prev_WP; // last waypoint
struct Location current_loc; // current location struct Location current_loc; // current location
struct Location next_WP; // next waypoint struct Location next_WP; // next waypoint
//struct Location tell_command; // command for telemetry struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded struct Location next_command; // command preloaded
long target_altitude; // used for long target_altitude; // used for
long offset_altitude; // used for long offset_altitude; // used for
@ -559,8 +559,10 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0)) if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0))
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();
#endif
if (g.log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
@ -608,8 +610,10 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor); Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor);
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_RAW) if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
#endif
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W #if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput(); readgcsinput();
@ -998,6 +1002,9 @@ void update_trig(void){
void update_alt() void update_alt()
{ {
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
altitude_sensor = BARO; altitude_sensor = BARO;
baro_alt = read_barometer(); baro_alt = read_barometer();
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt); //Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
@ -1029,6 +1036,7 @@ void update_alt()
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
} }
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt); //Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
#endif
// altitude smoothing // altitude smoothing
// ------------------ // ------------------

6
ArduCopterMega/GCS_Mavlink.pde
View File

@ -223,7 +223,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_ACTION_EMCY_KILL: case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN: case MAV_ACTION_SHUTDOWN:
set_mode(MANUAL); set_mode(ACRO);
break; break;
case MAV_ACTION_CONTINUE: case MAV_ACTION_CONTINUE:
@ -231,7 +231,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_ACTION_SET_MANUAL: case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL); set_mode(ACRO);
break; break;
case MAV_ACTION_SET_AUTO: case MAV_ACTION_SET_AUTO:
@ -265,7 +265,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_CALIBRATE_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground(); startup_ground();
break; break;
case MAV_ACTION_REC_START: break; case MAV_ACTION_REC_START: break;

101
ArduCopterMega/Mavlink_Common.h
View File

@ -1,3 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#ifndef Mavlink_Common_H #ifndef Mavlink_Common_H
#define Mavlink_Common_H #define Mavlink_Common_H
@ -39,40 +41,37 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
uint8_t nav_mode = MAV_NAV_VECTOR; uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) { switch(control_mode) {
case MANUAL: case ACRO:
mode = MAV_MODE_MANUAL; mode = MAV_MODE_MANUAL;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
break; break;
case STABILIZE: case STABILIZE:
mode = MAV_MODE_GUIDED; mode = MAV_MODE_GUIDED;
break; break;
case FLY_BY_WIRE_A: case FBW:
mode = MAV_MODE_TEST1; mode = MAV_MODE_TEST1;
break; break;
case FLY_BY_WIRE_B: case ALT_HOLD:
mode = MAV_MODE_TEST2; mode = MAV_MODE_TEST2;
break;
case POSITION_HOLD:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break; break;
case AUTO: case AUTO:
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT; nav_mode = MAV_NAV_WAYPOINT;
break; break;
case RTL: case RTL:
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING; nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break; break;
case TAKEOFF: case TAKEOFF:
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF; nav_mode = MAV_NAV_LIFTOFF;
break; break;
case LAND: case LAND:
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING; nav_mode = MAV_NAV_LANDING;
break; break;
} }
uint8_t status = MAV_STATE_ACTIVE; uint8_t status = MAV_STATE_ACTIVE;
@ -155,50 +154,40 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
uint8_t rssi = 1; uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000 // normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers // This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send( mavlink_msg_rc_channels_scaled_send(chan,
chan, g.rc_1.norm_output(),
10000 * g.channel_roll.norm_output(), g.rc_2.norm_output(),
10000 * g.channel_pitch.norm_output(), g.rc_3.norm_output(),
10000 * g.channel_throttle.norm_output(), g.rc_4.norm_output(),
10000 * g.channel_rudder.norm_output(), 0,0,0,0,rssi);
0,
0,
0,
0,
rssi);
break; break;
} }
case MSG_RADIO_IN: case MSG_RADIO_IN:
{ {
uint8_t rssi = 1; uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send( mavlink_msg_rc_channels_raw_send(chan,
chan, g.rc_1.radio_in,
g.channel_roll.radio_in, g.rc_2.radio_in,
g.channel_pitch.radio_in, g.rc_3.radio_in,
g.channel_throttle.radio_in, g.rc_4.radio_in,
g.channel_rudder.radio_in, g.rc_5.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined g.rc_6.radio_in,
g.rc_6.radio_in, g.rc_7.radio_in,
g.rc_7.radio_in, g.rc_8.radio_in,
g.rc_8.radio_in, rssi);
rssi);
break; break;
} }
case MSG_RADIO_OUT: case MSG_RADIO_OUT:
{ {
mavlink_msg_servo_output_raw_send( mavlink_msg_servo_output_raw_send(chan,
chan, motor_out[0],
g.channel_roll.radio_out, motor_out[1],
g.channel_pitch.radio_out, motor_out[2],
g.channel_throttle.radio_out, motor_out[3],
g.channel_rudder.radio_out, 0, 0, 0, 0);
g.rc_5.radio_out, // XXX currently only 4 RC channels defined break;
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
break;
} }
case MSG_VFR_HUD: case MSG_VFR_HUD:
@ -210,7 +199,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
dcm.yaw_sensor, dcm.yaw_sensor,
current_loc.alt / 100.0, current_loc.alt / 100.0,
climb_rate, climb_rate,
(int)g.channel_throttle.servo_out); nav_throttle);
break; break;
} }

2
ArduCopterMega/commands_logic.pde
View File

@ -189,7 +189,7 @@ verify_nav_wp()
// add in a more complex case // add in a more complex case
// Doug to do // Doug to do
if(loiter_sum > 300){ if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"Missed WP"); gcs.send_text(SEVERITY_MEDIUM,"Missed WP");
return true; return true;
} }
return false; return false;

5
ArduCopterMega/control_modes.pde
View File

@ -1,3 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
void read_control_switch() void read_control_switch()
{ {
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
@ -73,8 +75,9 @@ void read_trim_switch()
if(trim_flag){ if(trim_flag){
// switch was just released // switch was just released
if((millis() - trim_timer) > 2000){ if((millis() - trim_timer) > 2000){
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.save(); imu.save();
#endif
}else{ }else{
// set the throttle nominal // set the throttle nominal
if(g.rc_3.control_in > 50){ if(g.rc_3.control_in > 50){

1
ArduCopterMega/motors.pde
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#define ARM_DELAY 10 #define ARM_DELAY 10
#define DISARM_DELAY 10 #define DISARM_DELAY 10

12
ArduCopterMega/system.pde
View File

@ -115,8 +115,10 @@ void init_ardupilot()
init_rc_in(); // sets up rc channels from radio init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs init_rc_out(); // sets up the timer libs
init_camera(); init_camera();
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization barometer.Init(); // APM Abs Pressure sensor initialization
#endif
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
// Do GPS init // Do GPS init
@ -137,9 +139,11 @@ void init_ardupilot()
if(g.compass_enabled) if(g.compass_enabled)
init_compass(); init_compass();
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){ if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc); sonar.init(SONAR_PIN, &adc);
} }
#endif
pinMode(C_LED_PIN, OUTPUT); // GPS status LED pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED pinMode(A_LED_PIN, OUTPUT); // GPS status LED
@ -213,10 +217,12 @@ void startup_ground(void)
gcs.send_message(MSG_COMMAND_LIST, i); gcs.send_message(MSG_COMMAND_LIST, i);
} }
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets // Warm up and read Gyro offsets
// ----------------------------- // -----------------------------
imu.init_gyro(); imu.init_gyro();
report_imu(); report_imu();
#endif
// read the radio to set trims // read the radio to set trims
// --------------------------- // ---------------------------
@ -226,9 +232,11 @@ void startup_ground(void)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground // read Baro pressure at ground
//----------------------------- //-----------------------------
init_barometer(); init_barometer();
#endif
// initialize commands // initialize commands
// ------------------- // -------------------
@ -253,7 +261,7 @@ void set_mode(byte mode)
// disarm motors temp // disarm motors temp
motor_auto_safe = false; motor_auto_safe = false;
} }
//send_message(SEVERITY_LOW,"control mode"); //send_text(SEVERITY_LOW,"control mode");
//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
switch(control_mode) switch(control_mode)
{ {

9
ArduCopterMega/test.pde
View File

@ -47,7 +47,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"stabilize", test_stabilize}, {"stabilize", test_stabilize},
{"fbw", test_fbw}, {"fbw", test_fbw},
{"gps", test_gps}, {"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"adc", test_adc}, {"adc", test_adc},
#endif
{"imu", test_imu}, {"imu", test_imu},
//{"dcm", test_dcm}, //{"dcm", test_dcm},
//{"omega", test_omega}, //{"omega", test_omega},
@ -55,7 +57,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"current", test_current}, {"current", test_current},
{"relay", test_relay}, {"relay", test_relay},
{"waypoints", test_wp}, {"waypoints", test_wp},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"airpressure", test_pressure}, {"airpressure", test_pressure},
#endif
{"compass", test_mag}, {"compass", test_mag},
{"xbee", test_xbee}, {"xbee", test_xbee},
{"eedump", test_eedump}, {"eedump", test_eedump},
@ -396,6 +400,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv)
} }
} }
#if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t static int8_t
test_adc(uint8_t argc, const Menu::arg *argv) test_adc(uint8_t argc, const Menu::arg *argv)
{ {
@ -416,6 +421,8 @@ test_adc(uint8_t argc, const Menu::arg *argv)
} }
} }
} }
#endif
static int8_t static int8_t
test_imu(uint8_t argc, const Menu::arg *argv) test_imu(uint8_t argc, const Menu::arg *argv)
{ {
@ -769,6 +776,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
} }
} }
#if HIL_MODE != HIL_MODE_ATTITUDE
static int8_t static int8_t
test_pressure(uint8_t argc, const Menu::arg *argv) test_pressure(uint8_t argc, const Menu::arg *argv)
{ {
@ -830,6 +838,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
} }
} }
} }
#endif
static int8_t static int8_t
test_mag(uint8_t argc, const Menu::arg *argv) test_mag(uint8_t argc, const Menu::arg *argv)