39 #include <px4_platform_common/getopt.h> 40 #include <px4_platform_common/log.h> 60 ret = wait_until_running();
66 _task_id = task_id_is_work_queue;
73 if (_param_ev_tsk_stat_dis.get()) {
77 if (_param_ev_tsk_rc_loss.get()) {
108 PX4_ERR(
"alloc failed");
113 _object.store(send_event);
118 SendEvent *obj =
static_cast<SendEvent *
>(arg);
144 USEC2TICK(SEND_EVENT_INTERVAL_US));
157 bool got_temperature_calibration_command =
false, accel =
false, baro =
false, gyro =
false;
160 case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
161 if ((
int)(cmd.
param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
163 got_temperature_calibration_command =
true;
166 if ((
int)(cmd.
param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
168 got_temperature_calibration_command =
true;
171 if ((
int)(cmd.
param7) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
173 got_temperature_calibration_command =
true;
176 if (got_temperature_calibration_command) {
178 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
181 answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
195 command_ack.command = cmd.
command;
196 command_ack.result = (uint8_t)result;
201 command_ack_pub.publish(command_ack);
206 if (!strcmp(argv[0],
"temperature_calibration")) {
209 PX4_ERR(
"background task not running");
213 bool gyro_calib =
false, accel_calib =
false, baro_calib =
false;
214 bool calib_all =
true;
217 const char *myoptarg =
nullptr;
219 while ((ch = px4_getopt(argc, argv,
"abg", &myoptind, &myoptarg)) != EOF) {
244 vcmd.param1 = (float)((gyro_calib
245 || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
249 vcmd.param5 = ((accel_calib
250 || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : (
double)NAN);
251 vcmd.param6 = (double)NAN;
252 vcmd.param7 = (float)((baro_calib
253 || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
254 vcmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION;
257 vcmd_pub.publish(vcmd);
269 printf(
"%s\n\n", reason);
272 PRINT_MODULE_DESCRIPTION(
275 Background process running periodically on the LP work queue to perform housekeeping tasks. 276 It is currently only responsible for temperature calibration and tone alarm on RC Loss. 278 The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.). 281 PRINT_MODULE_USAGE_NAME("send_event",
"system");
282 PRINT_MODULE_USAGE_COMMAND_DESCR(
"start",
"Start the background task");
283 PRINT_MODULE_USAGE_COMMAND_DESCR(
"temperature_calibration",
"Run temperature calibration process");
284 PRINT_MODULE_USAGE_PARAM_FLAG(
'g',
"calibrate the gyro",
true);
285 PRINT_MODULE_USAGE_PARAM_FLAG(
'a',
"calibrate the accel",
true);
286 PRINT_MODULE_USAGE_PARAM_FLAG(
'b',
"calibrate the baro (if none of these is given, all will be calibrated)",
true);
287 PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
class manages the RC loss audible alarm, LED status display, and thermal calibration.
void answer_command(const vehicle_command_s &cmd, unsigned result)
Returns an ACK to a vehicle_command.
static int task_spawn(int argc, char *argv[])
Spawns and initializes the class in the same context as the work queue and starts the background list...
int send_event_main(int argc, char *argv[])
int main(int argc, char **argv)
rc_loss::RC_Loss_Alarm * _rc_loss_alarm
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
int run_temperature_calibration(bool accel, bool baro, bool gyro)
start temperature calibration in a new task for one or multiple sensors
void process_commands()
Checks for new commands and processes them.
int start()
Starts background task listening for commands.
void cycle()
Calls process_commands() and schedules the next cycle.
static int print_usage(const char *reason=nullptr)
Prints usage options to the console.
status::StatusDisplay * _status_display
const unsigned SEND_EVENT_INTERVAL_US
int get_vehicle_command_sub() const
bool vehicle_command_updated() const
static int custom_command(int argc, char *argv[])
Recognizes custom startup commands, called from the main() function entry.
void process()
regularily called to handle state updates
SubscriberHandler _subscriber_handler
static void cycle_trampoline(void *arg)
Process cycle trampoline for the work queue.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void process()
regularily called to handle state updates
static void initialize_trampoline(void *arg)
Trampoline for initialisation.