fix for Logging, motors_out buffer overrun, formatting.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1769 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-11 21:37:37 +00:00
parent 20fdbda315
commit 732446eb3a
8 changed files with 160 additions and 147 deletions

View File

@ -205,7 +205,7 @@ const char* flight_mode_strings[] = {
// Radio
// -----
int motor_out[4];
int motor_out[8];
Vector3f omega;
// Failsafe

View File

@ -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;
}
}

View File

@ -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")),

View File

@ -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
// ----------------

View File

@ -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

View File

@ -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();

View File

@ -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,"<startup_ground> GROUND START");
#if(GROUND_START_DELAY > 0)
gcs.send_text(SEVERITY_LOW,"<startup_ground> 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)

View File

@ -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,