zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

add trial period

mission-4.1.18
yaozb 5 years ago
parent
9df69543f0
commit
ecdc425ce8
10 changed files with 6476 additions and 6402 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1716
      ArduCopter/AP_Arming.cpp
  2. 124
      ArduCopter/AP_Arming.h
  3. 4
      ArduCopter/Copter.h
  4. 3281
      ArduCopter/Parameters.cpp
  5. 1268
      ArduCopter/Parameters.h
  6. 1385
      ArduCopter/system.cpp
  7. 3339
      libraries/AP_GPS/AP_GPS.cpp
  8. 1176
      libraries/AP_GPS/AP_GPS.h
  9. 583
      libraries/AP_GPS/GPS_Backend.cpp
  10. 2
      modules/mavlink

1716
ArduCopter/AP_Arming.cpp
View File

File diff suppressed because it is too large Load Diff

124
ArduCopter/AP_Arming.h
Unescape View File

@ -1,62 +1,62 @@ @@ -1,62 +1,62 @@
#pragma once
#include <AP_Arming/AP_Arming.h>
class AP_Arming_Copter : public AP_Arming
{
public:
friend class Copter;
friend class ToyMode;
AP_Arming_Copter() : AP_Arming()
{
// default REQUIRE parameter to 1 (Copter does not have an
// actual ARMING_REQUIRE parameter)
require.set_default((uint8_t)Required::YES_MIN_PWM);
}
/* Do not allow copies */
AP_Arming_Copter(const AP_Arming_Copter &other) = delete;
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
void update(void);
bool rc_calibration_checks(bool display_failure) override;
bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected:
bool pre_arm_checks(bool display_failure) override;
bool pre_arm_ekf_attitude_check();
bool pre_arm_terrain_check(bool display_failure);
bool proximity_checks(bool display_failure) const override;
bool arm_checks(AP_Arming::Method method) override;
// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(bool display_failure) override;
// NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override;
bool compass_checks(bool display_failure) override;
bool gps_checks(bool display_failure) override;
bool barometer_checks(bool display_failure) override;
bool board_voltage_checks(bool display_failure) override;
// NOTE! the following check functions *DO NOT* call into AP_Arming!
bool parameter_checks(bool display_failure);
bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);
bool oa_checks(bool display_failure);
bool mandatory_gps_checks(bool display_failure);
void set_pre_arm_check(bool b);
private:
// actually contains the pre-arm checks. This is wrapped so that
// we can store away success/failure of the checks.
bool run_pre_arm_checks(bool display_failure);
};
#pragma once
#include <AP_Arming/AP_Arming.h>
class AP_Arming_Copter : public AP_Arming
{
public:
friend class Copter;
friend class ToyMode;
AP_Arming_Copter() : AP_Arming()
{
// default REQUIRE parameter to 1 (Copter does not have an
// actual ARMING_REQUIRE parameter)
require.set_default((uint8_t)Required::YES_MIN_PWM);
}
/* Do not allow copies */
AP_Arming_Copter(const AP_Arming_Copter &other) = delete;
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
void update(void);
bool rc_calibration_checks(bool display_failure) override;
bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected:
bool pre_arm_checks(bool display_failure) override;
bool pre_arm_ekf_attitude_check();
bool pre_arm_terrain_check(bool display_failure);
bool proximity_checks(bool display_failure) const override;
bool arm_checks(AP_Arming::Method method) override;
// mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(bool display_failure) override;
// NOTE! the following check functions *DO* call into AP_Arming:
bool ins_checks(bool display_failure) override;
bool compass_checks(bool display_failure) override;
bool gps_checks(bool display_failure) override;
bool barometer_checks(bool display_failure) override;
bool board_voltage_checks(bool display_failure) override;
// NOTE! the following check functions *DO NOT* call into AP_Arming!
bool parameter_checks(bool display_failure);
bool motor_checks(bool display_failure);
bool pilot_throttle_checks(bool display_failure);
bool oa_checks(bool display_failure);
bool mandatory_gps_checks(bool display_failure);
bool mandatory_deadline_checks(bool display_failure);
void set_pre_arm_check(bool b);
private:
// actually contains the pre-arm checks. This is wrapped so that
// we can store away success/failure of the checks.
bool run_pre_arm_checks(bool display_failure);
};

4
ArduCopter/Copter.h
Unescape View File

@ -837,7 +837,7 @@ private: @@ -837,7 +837,7 @@ private:
void convert_lgr_parameters(void);
void convert_tradheli_parameters(void);
void convert_fs_options_params(void);
void get_deadline_params( int32_t &deadline );
// precision_landing.cpp
void init_precland();
void update_precland();
@ -895,7 +895,7 @@ private: @@ -895,7 +895,7 @@ private:
const char* get_frame_string();
void allocate_motors(void);
bool is_tradheli() const;
bool deadline_ok();
// terrain.cpp
void terrain_update();
void terrain_logging();

3281
ArduCopter/Parameters.cpp
View File

File diff suppressed because it is too large Load Diff

1268
ArduCopter/Parameters.h
View File

File diff suppressed because it is too large Load Diff

1385
ArduCopter/system.cpp
View File

File diff suppressed because it is too large Load Diff

3339
libraries/AP_GPS/AP_GPS.cpp
View File

File diff suppressed because it is too large Load Diff

1176
libraries/AP_GPS/AP_GPS.h
View File

File diff suppressed because it is too large Load Diff

583
libraries/AP_GPS/GPS_Backend.cpp
Unescape View File

@ -1,291 +1,292 @@ @@ -1,291 +1,292 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_GPS.h"
#include "GPS_Backend.h"
#include <AP_Logger/AP_Logger.h>
#define GPS_BACKEND_DEBUGGING 0
#if GPS_BACKEND_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
# define Debug(fmt, args ...)
#endif
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
port(_port),
gps(_gps),
state(_state)
{
state.have_speed_accuracy = false;
state.have_horizontal_accuracy = false;
state.have_vertical_accuracy = false;
}
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int32_t v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return u.v;
}
int16_t AP_GPS_Backend::swap_int16(int16_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
return u.v;
}
/**
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
*/
void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
year = bcd_date % 100;
mon = (bcd_date / 100) % 100;
day = bcd_date / 10000;
uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
// get time in seconds since unix epoch
uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
state.time_week = ret / AP_SEC_PER_WEEK;
state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC;
state.time_week_ms += msec;
}
/*
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
*/
void AP_GPS_Backend::fill_3d_velocity(void)
{
float gps_heading = radians(state.ground_course);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);
state.velocity.z = 0;
state.have_vertical_velocity = false;
}
void
AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
{
// not all backends have valid ports
if (port != nullptr) {
if (port->txspace() > len) {
port->write(data, len);
} else {
Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
}
}
}
void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
{
const uint8_t instance = state.instance;
const struct AP_GPS::detect_state dstate = gps.detect_state[instance];
if (dstate.auto_detected_baud) {
hal.util->snprintf(buffer, buflen,
"GPS %d: detected as %s at %d baud",
instance + 1,
name(),
gps._baudrates[dstate.current_baud]);
} else {
hal.util->snprintf(buffer, buflen,
"GPS %d: specified as %s",
instance + 1,
name());
}
}
void AP_GPS_Backend::broadcast_gps_type() const
{
#ifndef HAL_NO_GCS
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer);
#endif
}
void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
{
#ifndef HAL_NO_LOGGING
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
AP::logger().Write_Message(buffer);
#endif
}
bool AP_GPS_Backend::should_log() const
{
return gps.should_log();
}
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
#ifndef HAL_NO_GCS
const uint8_t instance = state.instance;
// send status
switch (instance) {
case 0:
mavlink_msg_gps_rtk_send(chan,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_week_number,
state.rtk_time_week_ms,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_num_sats,
state.rtk_baseline_coords_type,
state.rtk_baseline_x_mm,
state.rtk_baseline_y_mm,
state.rtk_baseline_z_mm,
state.rtk_accuracy,
state.rtk_iar_num_hypotheses);
break;
case 1:
mavlink_msg_gps2_rtk_send(chan,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_week_number,
state.rtk_time_week_ms,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_num_sats,
state.rtk_baseline_coords_type,
state.rtk_baseline_x_mm,
state.rtk_baseline_y_mm,
state.rtk_baseline_z_mm,
state.rtk_accuracy,
state.rtk_iar_num_hypotheses);
break;
}
#endif
}
/*
set a timestamp based on arrival time on uart at current byte,
assuming the message started nbytes ago
*/
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
{
if (port) {
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
}
}
void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
{
if (itow != _last_itow) {
_last_itow = itow;
/*
we need to calculate a pseudo-itow, which copes with the
iTow from the GPS changing in unexpected ways. We assume
that timestamps from the GPS are always in multiples of
50ms. That means we can't handle a GPS with an update rate
of more than 20Hz. We could do more, but we'd need the GPS
poll time to be higher
*/
const uint32_t gps_min_period_ms = 50;
// get the time the packet arrived on the UART
uint64_t uart_us = port->receive_time_constraint_us(msg_length);
uint32_t now = AP_HAL::millis();
uint32_t dt_ms = now - _last_ms;
_last_ms = now;
// round to nearest 50ms period
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
// work out an actual message rate. If we get 5 messages in a
// row with a new rate we switch rate
if (_last_rate_ms == dt_ms) {
if (_rate_counter < 5) {
_rate_counter++;
} else if (_rate_ms != dt_ms) {
_rate_ms = dt_ms;
}
} else {
_rate_counter = 0;
_last_rate_ms = dt_ms;
}
if (_rate_ms == 0) {
// only allow 5Hz to 20Hz in user config
_rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200);
}
// round to calculated message rate
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
// calculate pseudo-itow
_pseudo_itow += dt_ms * 1000U;
// use msg arrival time, and correct for jitter
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
state.uart_timestamp_ms = local_us / 1000U;
}
}
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_GPS.h"
#include "GPS_Backend.h"
#include <AP_Logger/AP_Logger.h>
#define GPS_BACKEND_DEBUGGING 0
#if GPS_BACKEND_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else
# define Debug(fmt, args ...)
#endif
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
port(_port),
gps(_gps),
state(_state)
{
state.have_speed_accuracy = false;
state.have_horizontal_accuracy = false;
state.have_vertical_accuracy = false;
}
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int32_t v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return u.v;
}
int16_t AP_GPS_Backend::swap_int16(int16_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
return u.v;
}
/**
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
*/
void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
year = bcd_date % 100;
mon = (bcd_date / 100) % 100;
day = bcd_date / 10000;
uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
state.zr_date_timestamp =(year+2000)*10000+mon*100+day; //FIXME only calcute onecs
// get time in seconds since unix epoch
uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
state.time_week = ret / AP_SEC_PER_WEEK;
state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC;
state.time_week_ms += msec;
}
/*
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
*/
void AP_GPS_Backend::fill_3d_velocity(void)
{
float gps_heading = radians(state.ground_course);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);
state.velocity.z = 0;
state.have_vertical_velocity = false;
}
void
AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len)
{
// not all backends have valid ports
if (port != nullptr) {
if (port->txspace() > len) {
port->write(data, len);
} else {
Debug("GPS %d: Not enough TXSPACE", state.instance + 1);
}
}
}
void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const
{
const uint8_t instance = state.instance;
const struct AP_GPS::detect_state dstate = gps.detect_state[instance];
if (dstate.auto_detected_baud) {
hal.util->snprintf(buffer, buflen,
"GPS %d: detected as %s at %d baud",
instance + 1,
name(),
gps._baudrates[dstate.current_baud]);
} else {
hal.util->snprintf(buffer, buflen,
"GPS %d: specified as %s",
instance + 1,
name());
}
}
void AP_GPS_Backend::broadcast_gps_type() const
{
#ifndef HAL_NO_GCS
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer);
#endif
}
void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const
{
#ifndef HAL_NO_LOGGING
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
_detection_message(buffer, sizeof(buffer));
AP::logger().Write_Message(buffer);
#endif
}
bool AP_GPS_Backend::should_log() const
{
return gps.should_log();
}
void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan)
{
#ifndef HAL_NO_GCS
const uint8_t instance = state.instance;
// send status
switch (instance) {
case 0:
mavlink_msg_gps_rtk_send(chan,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_week_number,
state.rtk_time_week_ms,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_num_sats,
state.rtk_baseline_coords_type,
state.rtk_baseline_x_mm,
state.rtk_baseline_y_mm,
state.rtk_baseline_z_mm,
state.rtk_accuracy,
state.rtk_iar_num_hypotheses);
break;
case 1:
mavlink_msg_gps2_rtk_send(chan,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_week_number,
state.rtk_time_week_ms,
0, // Not implemented yet
0, // Not implemented yet
state.rtk_num_sats,
state.rtk_baseline_coords_type,
state.rtk_baseline_x_mm,
state.rtk_baseline_y_mm,
state.rtk_baseline_z_mm,
state.rtk_accuracy,
state.rtk_iar_num_hypotheses);
break;
}
#endif
}
/*
set a timestamp based on arrival time on uart at current byte,
assuming the message started nbytes ago
*/
void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes)
{
if (port) {
state.uart_timestamp_ms = port->receive_time_constraint_us(nbytes) / 1000U;
}
}
void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
{
if (itow != _last_itow) {
_last_itow = itow;
/*
we need to calculate a pseudo-itow, which copes with the
iTow from the GPS changing in unexpected ways. We assume
that timestamps from the GPS are always in multiples of
50ms. That means we can't handle a GPS with an update rate
of more than 20Hz. We could do more, but we'd need the GPS
poll time to be higher
*/
const uint32_t gps_min_period_ms = 50;
// get the time the packet arrived on the UART
uint64_t uart_us = port->receive_time_constraint_us(msg_length);
uint32_t now = AP_HAL::millis();
uint32_t dt_ms = now - _last_ms;
_last_ms = now;
// round to nearest 50ms period
dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms;
// work out an actual message rate. If we get 5 messages in a
// row with a new rate we switch rate
if (_last_rate_ms == dt_ms) {
if (_rate_counter < 5) {
_rate_counter++;
} else if (_rate_ms != dt_ms) {
_rate_ms = dt_ms;
}
} else {
_rate_counter = 0;
_last_rate_ms = dt_ms;
}
if (_rate_ms == 0) {
// only allow 5Hz to 20Hz in user config
_rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200);
}
// round to calculated message rate
dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms;
// calculate pseudo-itow
_pseudo_itow += dt_ms * 1000U;
// use msg arrival time, and correct for jitter
uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us);
state.uart_timestamp_ms = local_us / 1000U;
}
}

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit d2cc7dbff67f8c318a40e1ef57a99488b4737fab
Subproject commit ff8ada062c96f04dc830e54b657aca4430408433
Write Preview
Loading…
Cancel
Save