From de18df06b507dbd62e079c234a91fb794b1cc96f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 14:00:49 +1000 Subject: [PATCH] GCS: make the two GCS links gcs0 and gcs3 the artifical separation between 'gcs' and 'hil' just leads to confusion. This also simplifies the code a bit more, and saves us a bit more text --- ArduPlane/ArduPlane.pde | 8 +++----- ArduPlane/Attitude.pde | 2 +- ArduPlane/GCS.h | 5 ----- ArduPlane/GCS_Mavlink.pde | 32 ++++++++++++++++++++++++++------ ArduPlane/Log.pde | 2 +- ArduPlane/commands.pde | 4 +--- ArduPlane/commands_logic.pde | 16 ++++++++-------- ArduPlane/commands_process.pde | 10 +++++----- ArduPlane/events.pde | 2 +- ArduPlane/navigation.pde | 3 +-- ArduPlane/planner.pde | 5 +++-- ArduPlane/sensors.pde | 2 +- ArduPlane/system.pde | 26 ++++++++++---------------- 13 files changed, 61 insertions(+), 56 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index c94ad0109e..81cad81a3b 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -164,8 +164,8 @@ AP_IMU_Shim imu; // never used // GCS selection //////////////////////////////////////////////////////////////////////////////// // -GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); -GCS_MAVLINK hil(Parameters::k_param_streamrates_port0); +GCS_MAVLINK gcs0(Parameters::k_param_streamrates_port0); +GCS_MAVLINK gcs3(Parameters::k_param_streamrates_port3); //////////////////////////////////////////////////////////////////////////////// // SONAR selection @@ -516,8 +516,7 @@ static void fast_loop() // XXX is it appropriate to be doing the comms below on the fast loop? - gcs.update(); - hil.update(); + gcs_update(); gcs_data_stream_send(45,1000); } @@ -704,7 +703,6 @@ static void update_GPS(void) // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { - SendDebugln("!! bad loc"); ground_start_count = 5; } else { diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 4847f86e76..0304ac7378 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -349,7 +349,7 @@ static void set_servos(void) static void demo_servos(byte i) { while(i > 0){ - gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); + gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS APM_RC.OutputCh(1, 1400); mavlink_delay(400); diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index b4a91c7638..7ad1fd9304 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -82,11 +82,6 @@ public: /// void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {} - /// Emit an update of the "current" waypoints, often previous, current and - /// next. - /// - void print_current_waypoints() {} - // // The following interfaces are not currently implemented as their counterparts // are not called in the mainline code. XXX ripe for re-specification. diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index edae129e91..01355bdbbf 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1115,8 +1115,7 @@ static void mavlink_delay(unsigned long t) } if (tnow - last_50hz > 20) { last_50hz = tnow; - gcs.update(); - hil.update(); + gcs_update(); } delay(1); } while (millis() - tstart < t); @@ -1129,8 +1128,8 @@ static void mavlink_delay(unsigned long t) */ static void gcs_send_message(enum ap_message id) { - gcs.send_message(id); - hil.send_message(id); + gcs0.send_message(id); + gcs3.send_message(id); } /* @@ -1138,6 +1137,27 @@ static void gcs_send_message(enum ap_message id) */ static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) { - gcs.data_stream_send(freqMin, freqMax); - hil.data_stream_send(freqMin, freqMax); + gcs0.data_stream_send(freqMin, freqMax); + gcs3.data_stream_send(freqMin, freqMax); +} + +/* + look for incoming commands on the GCS links + */ +static void gcs_update(void) +{ + gcs0.update(); + gcs3.update(); +} + +static void gcs_send_text(uint8_t severity, const char *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); +} + +static void gcs_send_text_P(uint8_t severity, const prog_char_t *str) +{ + gcs0.send_text(severity, str); + gcs3.send_text(severity, str); } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 59a9c66b0b..441be181c5 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -262,7 +262,7 @@ static void start_new_log(byte num_existing_logs) DataFlash.FinishWrite(); DataFlash.StartWrite(start_pages[num_existing_logs-1]); }else{ - gcs.send_text_P(SEVERITY_LOW,PSTR(" Logs full - logging discontinued")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" Logs full - logging discontinued")); } } diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index f40b14f2b0..4c89b3ba2a 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -135,7 +135,7 @@ It looks to see what the next command type is and finds the last command. */ static void set_next_WP(struct Location *wp) { - //gcs.send_text_P(SEVERITY_LOW,PSTR("load WP")); + //gcs_send_text_P(SEVERITY_LOW,PSTR("load WP")); SendDebug_P("MSG - wp_index: "); SendDebugln(g.waypoint_index, DEC); @@ -178,8 +178,6 @@ static void set_next_WP(struct Location *wp) // set a new crosstrack bearing // ---------------------------- reset_crosstrack(); - - gcs.print_current_waypoints(); } static void set_guided_WP(void) diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 55e636d584..e7d9d88505 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -63,7 +63,7 @@ handle_process_may() break; /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also) - gcs.send_text_P(SEVERITY_LOW,PSTR("Landing options set")); + gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 landing_pitch = next_command.lng * 100; @@ -160,7 +160,7 @@ static bool verify_must() break; default: - gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd")); return false; break; } @@ -185,7 +185,7 @@ static bool verify_may() break; default: - gcs.send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); break; } return false; @@ -320,13 +320,13 @@ static bool verify_nav_wp() //SendDebugln(command_must_index,DEC); char message[30]; sprintf(message,"Reached Waypoint #%i",command_must_index); - gcs.send_text(SEVERITY_LOW,message); + gcs_send_text(SEVERITY_LOW,message); return true; } // add in a more complex case // Doug to do if(loiter_sum > 300){ - gcs.send_text_P(SEVERITY_MEDIUM,PSTR(" Missed WP")); + gcs_send_text_P(SEVERITY_MEDIUM,PSTR(" Missed WP")); return true; } return false; @@ -344,7 +344,7 @@ static bool verify_loiter_time() update_loiter(); calc_bearing_error(); if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); return true; } return false; @@ -356,7 +356,7 @@ static bool verify_loiter_turns() calc_bearing_error(); if(loiter_sum > loiter_total) { loiter_total = 0; - gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); // clear the command queue; return true; } @@ -366,7 +366,7 @@ static bool verify_loiter_turns() static bool verify_RTL() { if (wp_distance <= g.waypoint_radius) { - gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); + gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); return true; }else{ return false; diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 719747bd13..35d3a02e64 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -6,7 +6,7 @@ static void change_command(uint8_t cmd_index) { struct Location temp = get_wp_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ - gcs.send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); + gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { command_must_index = NO_COMMAND; next_command.id = NO_COMMAND; @@ -59,7 +59,7 @@ static void load_next_command_from_EEPROM() // -------------------------------------------- if(next_command.id == NO_COMMAND){ // we are out of commands! - gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); handle_no_commands(); } } @@ -117,7 +117,7 @@ static void process_next_command() /**************************************************/ static void process_must() { - gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); + gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); // clear May indexes command_may_index = NO_COMMAND; @@ -133,7 +133,7 @@ static void process_must() static void process_may() { - gcs.send_text_P(SEVERITY_LOW,PSTR("")); + gcs_send_text_P(SEVERITY_LOW,PSTR("")); command_may_ID = next_command.id; handle_process_may(); @@ -151,6 +151,6 @@ static void process_now() // ----------------------------------------- next_command.id = NO_COMMAND; - gcs.send_text(SEVERITY_LOW, ""); + gcs_send_text(SEVERITY_LOW, ""); } diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index faa0e0dc69..c05079e496 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -78,7 +78,7 @@ static void failsafe_short_off_event() #if BATTERY_EVENT == ENABLED static void low_battery_event(void) { - gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); g.throttle_cruise = THROTTLE_CRUISE; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 487ffe4a3b..3c67117767 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -22,9 +22,8 @@ static void navigate() wp_distance = get_distance(¤t_loc, &next_WP); if (wp_distance < 0){ - gcs.send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); + gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); //Serial.println(wp_distance,DEC); - //print_current_waypoints(); return; } diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index 4b3f649b77..2dd749bbe3 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -25,14 +25,15 @@ planner_mode(uint8_t argc, const Menu::arg *argv) static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv) { - gcs.init(&Serial); + gcs0.init(&Serial); + gcs3.init(&Serial3); int loopcount = 0; while (1) { if (millis()-fast_loopTimer > 19) { fast_loopTimer = millis(); - gcs.update(); + gcs_update(); gcs_data_stream_send(45,1000); if ((loopcount % 5) == 0) // 10 hz gcs_data_stream_send(5,45); diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 3f7001d1ad..aeaeb6aacc 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -50,7 +50,7 @@ static void init_barometer(void) abs_pressure = ground_pressure; Serial.printf_P(PSTR("abs_pressure %ld\n"), abs_pressure); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("barometer calibration complete.")); } static long read_barometer(void) diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 4d4e3bdc7c..d983452d6a 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -159,19 +159,13 @@ static void init_ardupilot() g_gps->callback = mavlink_delay; // init the GCS -#if GCS_PORT == 3 - gcs.init(&Serial3); -#else - gcs.init(&Serial); -#endif + gcs0.init(&Serial); + gcs3.init(&Serial3); //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id mavlink_system.type = MAV_FIXED_WING; - // init the HIL - hil.init(&Serial); - rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs @@ -218,7 +212,7 @@ static void init_ardupilot() if (ENABLE_AIR_START == 1) { // Perform an air start and get back to flying - gcs.send_text_P(SEVERITY_LOW,PSTR(" AIR START")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" AIR START")); // Get necessary data from EEPROM //---------------- @@ -265,10 +259,10 @@ static void startup_ground(void) { set_mode(INITIALISING); - gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); #if(GROUND_START_DELAY > 0) - gcs.send_text_P(SEVERITY_LOW,PSTR(" With Delay")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" With Delay")); delay(GROUND_START_DELAY * 1000); #endif @@ -292,11 +286,11 @@ if (g.airspeed_enabled == true) // initialize airspeed sensor // -------------------------- zero_airspeed(); - gcs.send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" zero airspeed calibrated")); } else { - gcs.send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); + gcs_send_text_P(SEVERITY_LOW,PSTR(" NO airspeed")); } #endif @@ -327,7 +321,7 @@ else // ----------------------- demo_servos(3); - gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); + gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); } static void set_mode(byte mode) @@ -423,13 +417,13 @@ static void check_short_failsafe() static void startup_IMU_ground(void) { #if HIL_MODE != HIL_MODE_ATTITUDE - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); mavlink_delay(500); // Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // ----------------------- demo_servos(2); - gcs.send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); mavlink_delay(1000); imu.init(IMU::COLD_START, mavlink_delay);