diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 3456a5db3e..15b6c30a82 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -205,7 +205,7 @@ const char* flight_mode_strings[] = { // Radio // ----- -int motor_out[4]; +int motor_out[8]; Vector3f omega; // Failsafe diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index ce7252f06d..bc196d0f4f 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -52,19 +52,13 @@ GCS_MAVLINK::update(void) _queued_send(); // stop waypoint sending if timeout - if (global_data.waypoint_sending && - millis() - global_data.waypoint_timelast_send > - global_data.waypoint_send_timeout) - { + if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){ send_text(SEVERITY_LOW,"waypoint send timeout"); global_data.waypoint_sending = false; } // stop waypoint receiving if timeout - if (global_data.waypoint_receiving && - millis() - global_data.waypoint_timelast_receive > - global_data.waypoint_receive_timeout) - { + if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){ send_text(SEVERITY_LOW,"waypoint receive timeout"); global_data.waypoint_receiving = false; } @@ -74,7 +68,7 @@ void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax)) - send_message(MSG_RAW_IMU); + send_message(MSG_RAW_IMU); if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) { send_message(MSG_EXTENDED_STATUS); @@ -84,7 +78,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) - send_message(MSG_LOCATION); + send_message(MSG_LOCATION); if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers @@ -100,10 +94,9 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) send_message(MSG_ATTITUDE); if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info - send_message(MSG_VFR_HUD); + send_message(MSG_VFR_HUD); - if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)) - { + if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){ // Available datastream } } @@ -146,14 +139,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch(packet.req_stream_id) { case MAV_DATA_STREAM_ALL: - global_data.streamRateRawSensors = freq; - global_data.streamRateExtendedStatus = freq; - global_data.streamRateRCChannels = freq; - global_data.streamRateRawController = freq; - global_data.streamRatePosition = freq; - global_data.streamRateExtra1 = freq; - global_data.streamRateExtra2 = freq; - global_data.streamRateExtra3 = freq; + global_data.streamRateRawSensors = freq; + global_data.streamRateExtendedStatus = freq; + global_data.streamRateRCChannels = freq; + global_data.streamRateRawController = freq; + global_data.streamRatePosition = freq; + global_data.streamRateExtra1 = freq; + global_data.streamRateExtra2 = freq; + global_data.streamRateExtra3 = freq; break; case MAV_DATA_STREAM_RAW_SENSORS: global_data.streamRateRawSensors = freq; @@ -197,89 +190,88 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // do action send_text(SEVERITY_LOW,"action received"); - switch(packet.action) - { + switch(packet.action){ - case MAV_ACTION_LAUNCH: - //set_mode(TAKEOFF); - break; + case MAV_ACTION_LAUNCH: + //set_mode(TAKEOFF); + break; - case MAV_ACTION_RETURN: - set_mode(RTL); - break; + case MAV_ACTION_RETURN: + set_mode(RTL); + break; - case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); - break; + case MAV_ACTION_EMCY_LAND: + //set_mode(LAND); + break; - case MAV_ACTION_HALT: - do_hold_position(); - break; + case MAV_ACTION_HALT: + do_hold_position(); + break; - // can't implement due to APM configuration - // just setting to manual to be safe - case MAV_ACTION_MOTORS_START: - case MAV_ACTION_CONFIRM_KILL: - case MAV_ACTION_EMCY_KILL: - case MAV_ACTION_MOTORS_STOP: - case MAV_ACTION_SHUTDOWN: - set_mode(ACRO); - break; + // can't implement due to APM configuration + // just setting to manual to be safe + case MAV_ACTION_MOTORS_START: + case MAV_ACTION_CONFIRM_KILL: + case MAV_ACTION_EMCY_KILL: + case MAV_ACTION_MOTORS_STOP: + case MAV_ACTION_SHUTDOWN: + set_mode(ACRO); + break; - case MAV_ACTION_CONTINUE: - process_next_command(); - break; + case MAV_ACTION_CONTINUE: + process_next_command(); + break; - case MAV_ACTION_SET_MANUAL: - set_mode(ACRO); - break; + case MAV_ACTION_SET_MANUAL: + set_mode(ACRO); + break; - case MAV_ACTION_SET_AUTO: - set_mode(AUTO); - break; + case MAV_ACTION_SET_AUTO: + set_mode(AUTO); + break; - case MAV_ACTION_STORAGE_READ: - AP_Var::load_all(); - break; + case MAV_ACTION_STORAGE_READ: + AP_Var::load_all(); + break; - case MAV_ACTION_STORAGE_WRITE: - AP_Var::save_all(); - break; + case MAV_ACTION_STORAGE_WRITE: + AP_Var::save_all(); + break; - case MAV_ACTION_CALIBRATE_RC: break; - trim_radio(); - break; + case MAV_ACTION_CALIBRATE_RC: break; + trim_radio(); + break; - case MAV_ACTION_CALIBRATE_GYRO: - case MAV_ACTION_CALIBRATE_MAG: - case MAV_ACTION_CALIBRATE_ACC: - case MAV_ACTION_CALIBRATE_PRESSURE: - case MAV_ACTION_REBOOT: // this is a rough interpretation - startup_ground(); - break; + case MAV_ACTION_CALIBRATE_GYRO: + case MAV_ACTION_CALIBRATE_MAG: + case MAV_ACTION_CALIBRATE_ACC: + case MAV_ACTION_CALIBRATE_PRESSURE: + case MAV_ACTION_REBOOT: // this is a rough interpretation + startup_ground(); + break; - case MAV_ACTION_REC_START: break; - case MAV_ACTION_REC_PAUSE: break; - case MAV_ACTION_REC_STOP: break; + case MAV_ACTION_REC_START: break; + case MAV_ACTION_REC_PAUSE: break; + case MAV_ACTION_REC_STOP: break; - case MAV_ACTION_TAKEOFF: - //set_mode(TAKEOFF); - break; + case MAV_ACTION_TAKEOFF: + //set_mode(TAKEOFF); + break; - case MAV_ACTION_NAVIGATE: - set_mode(AUTO); - break; + case MAV_ACTION_NAVIGATE: + set_mode(AUTO); + break; - case MAV_ACTION_LAND: - //set_mode(LAND); - break; + case MAV_ACTION_LAND: + //set_mode(LAND); + break; - case MAV_ACTION_LOITER: - //set_mode(LOITER); - break; + case MAV_ACTION_LOITER: + //set_mode(LOITER); + break; - default: break; - } + default: break; + } break; } @@ -290,17 +282,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; // Start sending waypoints - mavlink_msg_waypoint_count_send(chan,msg->sysid, - msg->compid,g.waypoint_total + 1); // + home - global_data.waypoint_timelast_send = millis(); - global_data.waypoint_sending = true; - global_data.waypoint_receiving = false; - global_data.waypoint_dest_sysid = msg->sysid; - global_data.waypoint_dest_compid = msg->compid; - global_data.requested_interface = chan; + mavlink_msg_waypoint_count_send( + chan,msg->sysid, + msg->compid, + g.waypoint_total + 1); // + home + + global_data.waypoint_timelast_send = millis(); + global_data.waypoint_sending = true; + global_data.waypoint_receiving = false; + global_data.waypoint_dest_sysid = msg->sysid; + global_data.waypoint_dest_compid = msg->compid; + global_data.requested_interface = chan; break; } @@ -444,9 +441,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } g.waypoint_total.set_and_save(packet.count - 1); global_data.waypoint_timelast_receive = millis(); - global_data.waypoint_receiving = true; - global_data.waypoint_sending = false; - global_data.waypoint_request_i = 0; + global_data.waypoint_receiving = true; + global_data.waypoint_sending = false; + global_data.waypoint_request_i = 0; break; } @@ -520,7 +517,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (global_data.waypoint_request_i > g.waypoint_total) { uint8_t type = 0; // ok (0), error(1) - mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); + + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + type); + send_text(SEVERITY_LOW,"flight plan received"); global_data.waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can @@ -574,9 +577,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } // Report back new value - mavlink_msg_param_value_send(chan, (int8_t *)key, packet.param_value, - _count_parameters(), - -1); // XXX we don't actually know what its index is... + mavlink_msg_param_value_send( + chan, + (int8_t *)key, + packet.param_value, + _count_parameters(), + -1); // XXX we don't actually know what its index is... break; } // end case @@ -748,14 +754,10 @@ GCS_MAVLINK::_queued_send() AP_Var *vp; float value; - - // copy the current parameter and prepare to move to the next vp = _queued_parameter; _queued_parameter = _queued_parameter->next(); - - // if the parameter can be cast to float, report it here and break out of the loop value = vp->cast_to_float(); if (!isnan(value)) { @@ -763,11 +765,12 @@ GCS_MAVLINK::_queued_send() char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK vp->copy_name(param_name, sizeof(param_name)); - mavlink_msg_param_value_send(chan, - (int8_t*)param_name, - value, - _count_parameters(), - _queued_parameter_index); + mavlink_msg_param_value_send( + chan, + (int8_t*)param_name, + value, + _count_parameters(), + _queued_parameter_index); _queued_parameter_index++; break; @@ -783,12 +786,16 @@ GCS_MAVLINK::_queued_send() // request waypoints one by one // XXX note that this is pan-interface - if (global_data.waypoint_receiving && global_data.requested_interface == chan && - global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz + if (global_data.waypoint_receiving && + (global_data.requested_interface == chan) && + global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)) // limits to 3.33 hz { - mavlink_msg_waypoint_request_send(chan, - global_data.waypoint_dest_sysid, - global_data.waypoint_dest_compid ,global_data.waypoint_request_i); + mavlink_msg_waypoint_request_send( + chan, + global_data.waypoint_dest_sysid, + global_data.waypoint_dest_compid, + global_data.waypoint_request_i); + mavdelay = 0; } } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 6bd76a5d95..20f766999c 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -239,12 +239,12 @@ public: throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")), + flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")), flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")), pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), - log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")), + log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 7972a12bc1..6e7477f9c3 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -204,6 +204,7 @@ #define MASK_LOG_RAW (1<<7) #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CUR (1<<9) +#define MASK_LOG_SET_DEFAULTS (1<<15) // Waypoint Modes // ---------------- diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 11b1d558b7..95191decd4 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -279,20 +279,21 @@ set_servos_4() motor_auto_safe = true; } - // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - // fill the motor_out[] array for HIL use - for (unsigned char i=0; i<8; i++) { - motor_out[i] = 0; + for (unsigned char i = 0; i < 8; i++) { + motor_out[i] = g.rc_3.radio_min; } + // Send commands to motors + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + + if (g.frame_type == HEXA_FRAME) { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); } // reset I terms of PID controls diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 1fab699f5e..dd2306d832 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -99,6 +99,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) AP_Var::erase_all(); Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); + delay(1000); default_log_bitmask(); default_gains(); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 4ea60a4086..1e3fe0187f 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -141,7 +141,8 @@ void init_ardupilot() #endif - + if (g.log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); if(g.compass_enabled) init_compass(); @@ -199,6 +200,10 @@ void init_ardupilot() if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Startup(TYPE_GROUNDSTART_MSG); + if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { + default_log_bitmask(); + } + // set the correct flight mode // --------------------------- reset_control_switch(); @@ -212,7 +217,7 @@ void startup_ground(void) gcs.send_text(SEVERITY_LOW," GROUND START"); #if(GROUND_START_DELAY > 0) - gcs.send_text(SEVERITY_LOW," With Delay"); + //gcs.send_text(SEVERITY_LOW," With Delay"); delay(GROUND_START_DELAY * 1000); #endif @@ -302,9 +307,6 @@ void set_mode(byte mode) // output control mode to the ground station gcs.send_message(MSG_MODE_CHANGE); - if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); - } void set_failsafe(boolean mode) diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 0d7dc39f51..86d702aebd 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -20,7 +20,7 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); -static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); +static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -64,7 +64,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"compass", test_mag}, {"xbee", test_xbee}, {"eedump", test_eedump}, - {"rawgps", test_rawgps}, + {"rawgps", test_rawgps}, }; // A Macro to create the Menu @@ -768,20 +768,20 @@ test_wp_print(struct Location *cmd, byte index) static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + print_hit_enter(); + delay(1000); - while(1){ - if (Serial3.available()) - Serial1.write(Serial3.read()); + while(1){ + if (Serial3.available()) + Serial1.write(Serial3.read()); - if (Serial1.available()) - Serial3.write(Serial1.read()); + if (Serial1.available()) + Serial3.write(Serial1.read()); - if(Serial.available() > 0){ + if(Serial.available() > 0){ return (0); - } - } + } + } } static int8_t @@ -835,8 +835,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv) update_alt(); output_auto_throttle(); - Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"), + Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"), baro_alt, + sonar_alt, next_WP.alt, altitude_error, (int)g.throttle_cruise,