static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
{
#if defined RADIO_CONTROL
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
get_sys_time_msec(),
RC_CHANNELS,
PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
255); // rssi unknown
#else
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
get_sys_time_msec(),
0, // zero channels available
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
UINT16_MAX, UINT16_MAX, 255);
#endif
MAVLinkSendMessage();
}
static uint8_t gps_multi_switch(struct GpsState *gps_s) {
static uint32_t time_since_last_gps_switch = 0;
if (multi_gps_mode == GPS_MODE_PRIMARY){
return GpsInstances[PRIMARY_GPS_INSTANCE].id;
} else if (multi_gps_mode == GPS_MODE_SECONDARY){
return GpsInstances[SECONDARY_GPS_INSTANCE].id;
} else{
if (gps_s->fix > gps.fix){
return gps_s->comp_id;
} else if (gps.fix > gps_s->fix){
return gps.comp_id;
} else{
if (get_sys_time_msec() - time_since_last_gps_switch > TIME_TO_SWITCH) {
if (gps_s->num_sv > gps.num_sv) {
current_gps_id = gps_s->comp_id;
time_since_last_gps_switch = get_sys_time_msec();
} else if (gps.num_sv > gps_s->num_sv) {
current_gps_id = gps.comp_id;
time_since_last_gps_switch = get_sys_time_msec();
}
}
}
}
return current_gps_id;
}
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionNed_f()->x,
stateGetPositionNed_f()->y,
stateGetPositionNed_f()->z,
stateGetSpeedNed_f()->x,
stateGetSpeedNed_f()->y,
stateGetSpeedNed_f()->z);
MAVLinkSendMessage();
}
/**
* Send the attitude
*/
static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_attitude_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyEulers_f()->phi, // Phi
stateGetNedToBodyEulers_f()->theta, // Theta
stateGetNedToBodyEulers_f()->psi, // Psi
stateGetBodyRates_f()->p, // p
stateGetBodyRates_f()->q, // q
stateGetBodyRates_f()->r); // r
MAVLinkSendMessage();
}
static inline void mavlink_send_local_position_ned(void)
{
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionNed_f()->x,
stateGetPositionNed_f()->y,
stateGetPositionNed_f()->z,
stateGetSpeedNed_f()->x,
stateGetSpeedNed_f()->y,
stateGetSpeedNed_f()->z);
MAVLinkSendMessage();
}
/**
* Send the attitude
*/
static inline void mavlink_send_attitude(void)
{
mavlink_msg_attitude_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyEulers_f()->phi, // Phi
stateGetNedToBodyEulers_f()->theta, // Theta
stateGetNedToBodyEulers_f()->psi, // Psi
stateGetBodyRates_f()->p, // p
stateGetBodyRates_f()->q, // q
stateGetBodyRates_f()->r); // r
MAVLinkSendMessage();
}
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi,
stateGetNedToBodyQuat_f()->qx,
stateGetNedToBodyQuat_f()->qy,
stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r);
MAVLinkSendMessage();
}
static inline void mavlink_send_attitude_quaternion(void)
{
mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetNedToBodyQuat_f()->qi,
stateGetNedToBodyQuat_f()->qx,
stateGetNedToBodyQuat_f()->qy,
stateGetNedToBodyQuat_f()->qz,
stateGetBodyRates_f()->p,
stateGetBodyRates_f()->q,
stateGetBodyRates_f()->r);
MAVLinkSendMessage();
}
static void send_downlink(struct transport_tx *trans, struct link_device *dev) {
static uint32_t last_nb_bytes = 0;
// timestamp in usec when last message was send
static uint32_t last_ts = 0.;
// current timestamp
uint32_t now_ts = get_sys_time_msec();
// compute downlink byte rate
if (now_ts > last_ts) {
uint16_t rate = (1000 * ((uint32_t)downlink.nb_bytes - last_nb_bytes)) / (now_ts - last_ts);
last_ts = now_ts;
last_nb_bytes = downlink.nb_bytes;
// TODO uplink nb received msg
uint16_t uplink_nb_msgs = 0;
pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID,
&datalink_time, &uplink_nb_msgs,
&downlink.nb_msgs, &rate, &downlink.nb_ovrn);
}
}
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
{
float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
if (heading < 0.) {
heading += 360;
}
uint16_t compass_heading = heading * 100;
int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
/// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt,
relative_alt,
stateGetSpeedNed_f()->x * 100,
stateGetSpeedNed_f()->y * 100,
stateGetSpeedNed_f()->z * 100,
compass_heading);
MAVLinkSendMessage();
}
/**
* Send SYSTEM_TIME
* - time_unix_usec
* - time_boot_ms
*/
static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
{
mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
MAVLinkSendMessage();
}
/**
* Send SYSTEM_TIME
* - time_unix_usec
* - time_boot_ms
*/
static inline void mavlink_send_system_time(void)
{
mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
MAVLinkSendMessage();
}
void periodic_2Hz_openlog(void)
{
uint32_t timestamp = get_sys_time_msec();
DOWNLINK_SEND_TIMESTAMP(DefaultChannel, DefaultDevice, ×tamp);
}
void adc_isr(void)
#endif
{
uint8_t channel = 0;
uint16_t value = 0;
struct adc_buf * buf;
#if USE_ADC_WATCHDOG
/*
We need adc sampling fast enough to detect battery plug out, but we did not
need to get actual actual value so fast. So timer fire adc conversion fast,
at least 500 hz, but we inject adc value in sampling buffer only at 50hz
*/
const uint32_t timeStampDiff = get_sys_time_msec() - adc_watchdog.timeStamp;
const bool_t shouldAccumulateValue = timeStampDiff > 20;
if (shouldAccumulateValue)
adc_watchdog.timeStamp = get_sys_time_msec();
if (adc_watchdog.cb != NULL) {
if (adc_awd(adc_watchdog.adc)) {
ADC_SR(adc_watchdog.adc) &= ~ADC_SR_AWD; // clear int flag
adc_watchdog.cb();
}
}
#endif
#if USE_AD1
// Clear Injected End Of Conversion
if (adc_eoc_injected(ADC1)){
ADC_SR(ADC1) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
if (shouldAccumulateValue) {
#endif
for (channel = 0; channel < nb_adc1_channels; channel++) {
buf = adc1_buffers[channel];
if (buf) {
value = adc_read_injected(ADC1, channel+1);
adc_push_sample(buf, value);
}
}
#if USE_ADC_WATCHDOG
}
#endif
#if !USE_AD2 && !USE_AD3
adc_new_data_trigger = TRUE;
#endif
}
#endif
#if USE_AD2
if (adc_eoc_injected(ADC2)){
ADC_SR(ADC2) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
if (shouldAccumulateValue) {
#endif
for (channel = 0; channel < nb_adc2_channels; channel++) {
buf = adc2_buffers[channel];
if (buf) {
value = adc_read_injected(ADC2, channel+1);
adc_push_sample(buf, value);
}
}
#if USE_ADC_WATCHDOG
}
#endif
#if !USE_AD3
adc_new_data_trigger = TRUE;
#endif
}
#endif
#if USE_AD3
if (adc_eoc_injected(ADC3)){
ADC_SR(ADC3) &= ~ADC_SR_JEOC;
#if USE_ADC_WATCHDOG
if (shouldAccumulateValue) {
#endif
for (channel = 0; channel < nb_adc3_channels; channel++) {
buf = adc3_buffers[channel];
if (buf) {
value = adc_read_injected(ADC3, channel+1);
adc_push_sample(buf, value);
}
}
#if USE_ADC_WATCHDOG
}
#endif
adc_new_data_trigger = TRUE;
}
#endif
return;
}
