From b1e54558e4ab6d19feddcfe65f80429a793c89bb Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Thu, 3 Mar 2011 11:39:52 +0000 Subject: [PATCH] mavlink: fixed build with MAVLink and HIL 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 --- ArduCopterMega/ArduCopterMega.pde | 12 +++- ArduCopterMega/GCS_Mavlink.pde | 6 +- ArduCopterMega/Mavlink_Common.h | 101 +++++++++++++----------------- ArduCopterMega/commands_logic.pde | 2 +- ArduCopterMega/control_modes.pde | 5 +- ArduCopterMega/motors.pde | 1 + ArduCopterMega/system.pde | 12 +++- ArduCopterMega/test.pde | 9 +++ 8 files changed, 83 insertions(+), 65 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 6fff40cfa1..8df5062431 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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(); -} \ No newline at end of file +} diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 4777ff6fc4..1b995a633f 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -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; diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index f6a449b3ec..88fccf621b 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -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; } diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 904d9d0973..9d30f3e314 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -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; diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 6fc8da9614..717ddb45ec 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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){ diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index ddeed010c9..01ff15d996 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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 diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index fc18e96a79..566d399f6c 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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) { diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 9decd17c1a..c657cba172 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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)