Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues 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 b59474fc1a
commit b1e54558e4
8 changed files with 83 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopterMega/ArduCopterMega.pde
View File

@ -360,7 +360,7 @@ struct Location home; // home location
struct Location prev_WP; // last waypoint
struct Location current_loc; // current location
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
long target_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))
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)
Log_Write_Control_Tuning();
#endif
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
@ -608,8 +610,10 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
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)
Log_Write_Raw();
#endif
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W
readgcsinput();
@ -998,6 +1002,9 @@ void update_trig(void){
void update_alt()
{
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
altitude_sensor = BARO;
baro_alt = read_barometer();
//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;
}
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
#endif
// altitude smoothing
// ------------------
@ -1037,4 +1045,4 @@ void update_alt()
// Amount of throttle to apply for hovering
// ----------------------------------------
calc_nav_throttle();
}
}

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_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
set_mode(MANUAL);
set_mode(ACRO);
break;
case MAV_ACTION_CONTINUE:
@ -231,7 +231,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_SET_MANUAL:
set_mode(MANUAL);
set_mode(ACRO);
break;
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_PRESSURE:
case MAV_ACTION_REBOOT: // this is a rough interpretation
startup_IMU_ground();
startup_ground();
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
#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;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
case ACRO:
mode = MAV_MODE_MANUAL;
break;
case STABILIZE:
mode = MAV_MODE_GUIDED;
mode = MAV_MODE_GUIDED;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST1;
case FBW:
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
case ALT_HOLD:
mode = MAV_MODE_TEST2;
break;
case POSITION_HOLD:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case TAKEOFF:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
break;
case LAND:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
break;
}
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;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
mavlink_msg_rc_channels_scaled_send(chan,
g.rc_1.norm_output(),
g.rc_2.norm_output(),
g.rc_3.norm_output(),
g.rc_4.norm_output(),
0,0,0,0,rssi);
break;
}
case MSG_RADIO_IN:
{
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
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);
break;
}
case MSG_RADIO_OUT:
{
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
break;
mavlink_msg_servo_output_raw_send(chan,
motor_out[0],
motor_out[1],
motor_out[2],
motor_out[3],
0, 0, 0, 0);
break;
}
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,
current_loc.alt / 100.0,
climb_rate,
(int)g.channel_throttle.servo_out);
nav_throttle);
break;
}

2
ArduCopterMega/commands_logic.pde
View File

@ -189,7 +189,7 @@ verify_nav_wp()
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"Missed WP");
gcs.send_text(SEVERITY_MEDIUM,"Missed WP");
return true;
}
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()
{
byte switchPosition = readSwitch();
@ -73,8 +75,9 @@ void read_trim_switch()
if(trim_flag){
// switch was just released
if((millis() - trim_timer) > 2000){
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.save();
#endif
}else{
// set the throttle nominal
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 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_out(); // sets up the timer libs
init_camera();
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization
#endif
DataFlash.Init(); // DataFlash log initialization
// Do GPS init
@ -137,9 +139,11 @@ void init_ardupilot()
if(g.compass_enabled)
init_compass();
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
sonar.init(SONAR_PIN, &adc);
}
#endif
pinMode(C_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);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
report_imu();
#endif
// read the radio to set trims
// ---------------------------
@ -226,9 +232,11 @@ void startup_ground(void)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
// initialize commands
// -------------------
@ -253,7 +261,7 @@ void set_mode(byte mode)
// disarm motors temp
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);
switch(control_mode)
{

9
ArduCopterMega/test.pde
View File

@ -47,7 +47,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"stabilize", test_stabilize},
{"fbw", test_fbw},
{"gps", test_gps},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"adc", test_adc},
#endif
{"imu", test_imu},
//{"dcm", test_dcm},
//{"omega", test_omega},
@ -55,7 +57,9 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"current", test_current},
{"relay", test_relay},
{"waypoints", test_wp},
#if HIL_MODE != HIL_MODE_ATTITUDE
{"airpressure", test_pressure},
#endif
{"compass", test_mag},
{"xbee", test_xbee},
{"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
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
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
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
test_mag(uint8_t argc, const Menu::arg *argv)