mirror of https://github.com/ArduPilot/ardupilot
Rover: log the X accel that triggers auto
This commit is contained in:
parent
1d983b3f2e
commit
a88ac50e6c
|
@ -119,6 +119,7 @@ static Parameters g;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// prototypes
|
||||
static void update_events(void);
|
||||
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// DataFlash
|
||||
|
|
|
@ -9,9 +9,6 @@ static bool mavlink_active;
|
|||
// check if a message will fit in the payload space available
|
||||
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
|
||||
|
||||
// prototype this for use inside the GCS class
|
||||
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||
|
||||
/*
|
||||
* !!NOTE!!
|
||||
*
|
||||
|
|
|
@ -42,13 +42,16 @@ static bool auto_check_trigger(void)
|
|||
if (g.auto_trigger_pin != -1) {
|
||||
hal.gpio->pinMode(g.auto_trigger_pin, GPIO_INPUT);
|
||||
if (hal.gpio->read(g.auto_trigger_pin) == 0) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
|
||||
auto_triggered = true;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
if (g.auto_kickstart != 0.0f) {
|
||||
if (ins.get_accel().x >= g.auto_kickstart) {
|
||||
float xaccel = ins.get_accel().x;
|
||||
if (xaccel >= g.auto_kickstart) {
|
||||
gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
|
||||
auto_triggered = true;
|
||||
return true;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue