aboutsummaryrefslogtreecommitdiff
path: root/apps/commander/commander.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r--apps/commander/commander.c82
1 files changed, 49 insertions, 33 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 19fc81f63..149a662fd 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -95,10 +95,7 @@ static int leds;
static int buzzer;
static int mavlink_fd;
static bool commander_initialized = false;
-static struct vehicle_status_s current_status = {
- .state_machine = SYSTEM_STATE_PREFLIGHT,
- .mode = 0
-}; /**< Main state machine */
+static struct vehicle_status_s current_status; /**< Main state machine */
static int stat_pub;
static uint16_t nofix_counter = 0;
@@ -306,8 +303,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
- mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
- break;
+ //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
+ //break;
}
}
@@ -376,9 +373,22 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
printf("\nFINAL:\nmag min: %d\t%d\t%d\nmag max: %d\t%d\t%d\n", (int)min_avg[0], (int)min_avg[1], (int)min_avg[2], (int)max_avg[0], (int)max_avg[1], (int)max_avg[2]);
float mag_offset[3];
- mag_offset[0] = (max_avg[0] - min_avg[0])/2;
- mag_offset[1] = (max_avg[1] - min_avg[1])/2;
- mag_offset[2] = (max_avg[2] - min_avg[2])/2;
+
+ /**
+ * The offset is subtracted from the sensor values, so the result is the
+ * POSITIVE number that has to be subtracted from the sensor data
+ * to shift the center to zero
+ *
+ * offset = max - ((max - min) / 2.0f)
+ *
+ * which reduces to
+ *
+ * offset = (max + min) / 2.0f
+ */
+
+ mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f;
+ mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f;
+ mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f;
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_XOFFSET] = mag_offset[0];
global_data_parameter_storage->pm.param_values[PARAM_SENSOR_MAG_YOFFSET] = mag_offset[1];
@@ -532,8 +542,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
+ mavlink_log_info(mavlink_fd, "[commander] CMD starting gyro calibration");
do_gyro_calibration(status_pub, &current_status);
+ mavlink_log_info(mavlink_fd, "[commander] CMD finished gyro calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
@@ -549,12 +560,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
- mavlink_log_info(mavlink_fd, "[commander] starting mag calibration");
+ mavlink_log_info(mavlink_fd, "[commander] CMD starting mag calibration");
do_mag_calibration(status_pub, &current_status);
+ mavlink_log_info(mavlink_fd, "[commander] CMD finished mag calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;
} else {
- mavlink_log_critical(mavlink_fd, "[commander] REJECTING mag calibration");
+ mavlink_log_critical(mavlink_fd, "[commander] CMD REJECTING mag calibration");
result = MAV_RESULT_DENIED;
}
handled = true;
@@ -562,8 +574,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* none found */
if (!handled) {
- fprintf(stderr, "[commander] refusing unsupported calibration request\n");
- mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request");
+ //fprintf(stderr, "[commander] refusing unsupported calibration request\n");
+ mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request");
result = MAV_RESULT_UNSUPPORTED;
}
}
@@ -576,12 +588,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
if (((int)cmd->param1) == 0) {
if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
- printf("[commander] Loaded EEPROM params in RAM\n");
- mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM");
+ //printf("[commander] Loaded EEPROM params in RAM\n");
+ mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM");
result = MAV_RESULT_ACCEPTED;
} else {
- fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
+ //fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
result = MAV_RESULT_FAILED;
}
@@ -591,18 +603,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} else if (((int)cmd->param1) == 1) {
if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
- printf("[commander] RAM params written to EEPROM\n");
+ //printf("[commander] RAM params written to EEPROM\n");
mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
result = MAV_RESULT_ACCEPTED;
} else {
- fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
+ //fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
result = MAV_RESULT_FAILED;
}
} else {
- fprintf(stderr, "[commander] refusing unsupported storage request\n");
+ //fprintf(stderr, "[commander] refusing unsupported storage request\n");
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
result = MAV_RESULT_UNSUPPORTED;
}
@@ -622,8 +634,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
/* send any requested ACKs */
if (cmd->confirmation > 0) {
/* send acknowledge command */
- mavlink_message_t msg;
- mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
}
}
@@ -785,9 +795,11 @@ int commander_main(int argc, char *argv[])
/* make sure we are in preflight state */
memset(&current_status, 0, sizeof(current_status));
current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
+ current_status.flag_system_armed = false;
/* advertise to ORB */
stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+ /* publish current state machine */
state_machine_publish(stat_pub, &current_status, mavlink_fd);
if (stat_pub < 0) {
@@ -874,9 +886,9 @@ int commander_main(int argc, char *argv[])
battery_voltage_valid = sensors.battery_voltage_valid;
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
- flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+// flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
- /* Slow but important 5 Hz checks */
+ /* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
@@ -891,8 +903,8 @@ int commander_main(int argc, char *argv[])
}
/* toggle error led at 5 Hz in HIL mode */
- if ((current_status.mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
- /* armed */
+ if (current_status.flag_hil_enabled) {
+ /* hil enabled */
led_toggle(LED_AMBER);
} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
@@ -994,14 +1006,14 @@ int commander_main(int argc, char *argv[])
//
//
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
- update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
+ //update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
- /* if battery voltage is getting lower, warn using buzzer, etc. */
+ /* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
@@ -1086,8 +1098,8 @@ int commander_main(int argc, char *argv[])
} else {
static uint64_t last_print_time = 0;
- /* print error message for first RC glitch and then every 2 s / 2000 ms) */
- if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 3000000)) {
+ /* print error message for first RC glitch and then every 5 s / 5000 ms) */
+ if (!current_status.rc_signal_lost || ((hrt_absolute_time() - last_print_time) > 5000000)) {
mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO REMOTE SIGNAL!");
last_print_time = hrt_absolute_time();
}
@@ -1105,18 +1117,22 @@ int commander_main(int argc, char *argv[])
current_status.counter++;
current_status.timestamp = hrt_absolute_time();
- if (voltage_previous != current_status.voltage_battery) orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
/* If full run came back clean, transition to standby */
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
current_status.preflight_gyro_calibration == false &&
current_status.preflight_mag_calibration == false) {
/* All ok, no calibration going on, go to standby */
- do_state_update(stat_pub, &current_status, SYSTEM_STATE_STANDBY, mavlink_fd);
+ do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
+ }
+
+ /* publish at least with 1 Hz */
+ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+ orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
}
/* Store old modes to detect and act on state transitions */
- // vehicle_state_previous = current_status.state_machine;
voltage_previous = current_status.voltage_battery;
fflush(stdout);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-05 01:14:28 +0000