Exemplo n.º 1
0
/**@brief Function for decoding received data page from the ANT channel. 
 *
 * @param[in] p_event_message_buffer ANT event message buffer.
 * @param[out] p_event_return        Output data from the profile. 
 *
 * @return true if p_event_return contains valid data, false otherwise.
 */
static __INLINE bool rx_data_page_decode(uint8_t const * const p_event_message_buffer,
                                         antplus_event_return_t * p_event_return)
{
    // Variables required by the CTF cadence calculation.
    static uint32_t     ctf_previous_event_count   = 0;
    static uint32_t     ctf_previous_time_stamp             = 0;
    static uint32_t     ctf_zero_speed_counter              = 0;
    static bool         ctf_first_page_check_active         = true;
    uint32_t            ctf_event_count_difference;
    uint32_t            ctf_time_stamp_difference;
    
    // References to data pages.
    bp_page16_data_t *  p_page16_data;
    bp_page17_data_t *  p_page17_data;    
    bp_page18_data_t *  p_page18_data;        
    bp_page32_data_t *  p_page32_data;   
    page80_data_t *     p_page80_data;
    page81_data_t *     p_page81_data;    
    
    bool return_code        = true;        
    const uint32_t page_id  = p_event_message_buffer[BUFFER_INDEX_MESG_DATA];        
    p_event_return->event   = ANTPLUS_EVENT_PAGE;
    p_event_return->param1  = page_id;    
            
    switch (page_id)
    {        
        case BP_PAGE_1: 
            // Calibration message main data page.
            return_code = calibration_data_page_decode(p_event_message_buffer);
            break;

        case BP_PAGE_16:    
            // Standard power only page.
            p_page16_data = bp_rx_data_page16_get();    
        
            p_page16_data->event_count = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 1u];
            p_page16_data->pedal_power = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 2u];
            p_page16_data->instantaneous_cadence 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u];
            // Reset to zero prior bit-masking.
            p_page16_data->accumulated_power = 0;   
            // LSB. 
            p_page16_data->accumulated_power 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u];          
            // MSB. 
            p_page16_data->accumulated_power        
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u]) << 8u; 
            // Reset to zero prior bit-masking.
            p_page16_data->instantaneous_power = 0; 
             // LSB. 
            p_page16_data->instantaneous_power        
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u];         
            // MSB. 
            p_page16_data->instantaneous_power        
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u]) << 8u; 
            break; 

        case BP_PAGE_17: 
            // Wheel Torque (WT) main data page.
            p_page17_data = bp_rx_data_page17_get(); 

            p_page17_data->update_event_counter 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 1u]; 
            p_page17_data->wheel_ticks 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 2u]; 
            p_page17_data->instantaneous_cadence 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u]; 
            // Reset to zero prior bit-masking.
            p_page17_data->wheel_period = 0;        
            // LSB. 
            p_page17_data->wheel_period 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u];          
            // MSB. 
            p_page17_data->wheel_period 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u]) << 8u; 
            // LSB. 
            p_page17_data->accumulated_torgue 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u];          
            // MSB. 
            p_page17_data->accumulated_torgue 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u]) << 8u; 
            break; 

        case BP_PAGE_18: 
            // Standard Crank Torque (CT) main data page.      
            p_page18_data = bp_rx_data_page18_get(); 
        
            p_page18_data->update_event_counter 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 1u]; 
            p_page18_data->crank_ticks 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 2u]; 
            p_page18_data->instantaneous_cadence 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u]; 
            // Reset to zero prior bit-masking.
            p_page18_data->crank_period = 0; 
            // LSB.
            p_page18_data->crank_period 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u];            
            // MSB.  
            p_page18_data->crank_period 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u]) << 8u; 
            // LSB.  
            p_page18_data->accumulated_torgue 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u];          
            // MSB. 
            p_page18_data->accumulated_torgue 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u]) << 8u;  
            break;

        case BP_PAGE_32: 
            // Standard Crank Torque Frequency (CTF) main data page. 
            // @note: CTF messages are big endian. The byte order for these messages is reversed 
            //        with respect to standard ANT+ messages. 

            p_page32_data = bp_rx_data_page32_get(); 
        
            p_page32_data->update_event_counter 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 1u]; 
            // Reset to zero prior bit-masking.
            p_page32_data->slope = 0;               
            // MSB. 
            p_page32_data->slope
                = (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 2u]) << 8u;  
            // LSB. 
            p_page32_data->slope
                |= p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u];         
            // Reset to zero prior bit-masking.
            p_page32_data->time_stamp = 0;          
            // MSB. 
            p_page32_data->time_stamp
                = (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u]) << 8u;  
            // LSB. 
            p_page32_data->time_stamp
                |= p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u];         
            // Reset to zero prior bit-masking.
            p_page32_data->torque_ticks_stamp = 0;  
            // MSB. 
            p_page32_data->torque_ticks_stamp
                = (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u]) << 8u;  
            
            p_page32_data->torque_ticks_stamp
                |= p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u];         

            // Calculate average cadence based on received data, if feasible.
            
            // Store N - (N-1) difference, where N is the current received event count.
            ctf_event_count_difference  
                = (p_page32_data->update_event_counter - ctf_previous_event_count) 
                & UINT8_MAX;     
            // Store N - (N-1) difference, where N is time of most recent received rotation event. 
            ctf_time_stamp_difference 
                = (p_page32_data->time_stamp - ctf_previous_time_stamp) & UINT16_MAX; 

            if (!ctf_first_page_check_active)    // This is NOT the 1st received CTF page.
            {
                if (ctf_event_count_difference == 0)    // No new cadence event exist.
                {
                    ++ctf_zero_speed_counter;
                    if (ctf_zero_speed_counter == CTF_AVERAGE_CADENCE_ZERO_THRESHOLD)
                    {
                        p_page32_data->average_cadence = 0; // Zero-speed threshold reached.
                    } 
                }
                else                                    // New cadence event exists.
                {
                    ctf_zero_speed_counter = 0;         // Reset zero-speed check counter.
                    if (ctf_time_stamp_difference != 0) // Time stamp difference exists
                    {
                        // Calculate new average cadence based on the received data.
                        p_page32_data->average_cadence = 
                            ((ctf_event_count_difference << CTF_AVERAGE_CADENCE_SHIFT_MAGIC) 
                            * CTF_AVERAGE_CADENCE_MULTIPLY_MAGIC) / ctf_time_stamp_difference;  
                        
                        // Update previous value counters to be ready for the next received message.
                        ctf_previous_event_count   = p_page32_data->update_event_counter;
                        ctf_previous_time_stamp             = p_page32_data->time_stamp; 
                    }
                    else                                // Time stamp difference does not exist.
                    {
                        // Reaching this execution path would imply a broken sensor implementation 
                        // as having event count difference without time stamp difference. This 
                        // will be silently discarded. 
                    }
                }                
            }
            else    
            // This is the 1st received CTF page - no calculations are possible.
            {
                // Disable 1st received CTF page check.
                ctf_first_page_check_active         = false;    
                
                // Update previous value counters to be ready for the next received message.
                ctf_previous_event_count   = p_page32_data->update_event_counter;
                ctf_previous_time_stamp             = p_page32_data->time_stamp;
            }
            break;

        case COMMON_PAGE_80: 
            // Manufacturer's identification common data page.
            p_page80_data = bp_rx_data_page80_get();
        
            p_page80_data->hw_revision 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u]; 
            // LSB. 
            p_page80_data->manufacturing_id 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u];          
            // MSB. 
            p_page80_data->manufacturing_id 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u]) << 8u; 
            // LSB. 
            p_page80_data->model_number 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u];          
            // MSB.
            p_page80_data->model_number 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u]) << 8u; 
            break;

        case COMMON_PAGE_81: 
            // Product information common data page.        
            p_page81_data = bp_rx_data_page81_get();    
        
            p_page81_data->sw_revision 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 3u];    
            // Bits 0 - 7. 
            p_page81_data->serial_number 
                = p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 4u];          
            // Bits 8 - 15.
            p_page81_data->serial_number 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 5u]) << 8u; 
            // Bits 16 - 23.
            p_page81_data->serial_number 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 6u]) << 16u;
            // Bits 24 - 31. 
            p_page81_data->serial_number 
                |= (p_event_message_buffer[BUFFER_INDEX_MESG_DATA + 7u]) << 24u;
            break;

        default:
            return_code = false;
            break;
    }

    return return_code;    
}
/**@brief Function for processing received profile page events.
 *
 * @param[in] p_event_return Profile page event to be processed.
 */
static __INLINE void profile_event_page_handle(const antplus_event_return_t * const p_event_return)
{
#if defined(TRACE_DATA_PAGE)
    const bp_page16_data_t * p_page16_data;
    const bp_page17_data_t * p_page17_data;
    const bp_page18_data_t * p_page18_data;
    const bp_page32_data_t * p_page32_data;
    const page80_data_t    * p_page80_data;
    const page81_data_t    * p_page81_data;
#endif // defined(TRACE_DATA_PAGE)
#if defined(TRACE_CALIBRATION)
    const bp_page1_response_data_t * p_page1_general_response_data;
#endif // defined(TRACE_CALIBRATION)

    switch (p_event_return->param1)
    {
        case BP_PAGE_1:
            // Calibration message main data page.
#if defined(TRACE_CALIBRATION)
            p_page1_general_response_data = bp_rx_data_page1_response_get();

            printf("page1:calibration_id %#x\n",
                   (uint8_t)p_page1_general_response_data->calibration_id);
            printf("page1:auto_zero_status %u\n",
                   (uint8_t)p_page1_general_response_data->auto_zero_status);
            printf("page1:calibration_data %i\n",
                   (uint16_t)p_page1_general_response_data->calibration_data);
#endif // defined(TRACE_CALIBRATION)
            break;

        case BP_PAGE_16:
            // Standard power only page.
#if defined(TRACE_DATA_PAGE)
            p_page16_data = bp_rx_data_page16_get();

            printf("Page16:event count %u\n", (unsigned int)p_page16_data->event_count);
            printf("Page16:pedal power %u\n", (unsigned int)p_page16_data->pedal_power);
            printf("Page16:instantaneous cadence %u\n", (unsigned int)p_page16_data->instantaneous_cadence);
            printf("Page16:accumulated power %u\n", (unsigned int)p_page16_data->accumulated_power);
            printf("Page16:instantaneous power %u\n", (unsigned int)p_page16_data->instantaneous_power);
#endif // defined(TRACE_DATA_PAGE)     
            break;

        case BP_PAGE_17:
            // Wheel Torque (WT) main data page.
#if defined(TRACE_DATA_PAGE)
            p_page17_data = bp_rx_data_page17_get();

            printf("Page17:update_event_counter %u\n", (unsigned int)p_page17_data->update_event_counter);
            printf("Page17:wheel_ticks %u\n", (unsigned int)p_page17_data->wheel_ticks);
            printf("Page17:instantaneous_cadence %u\n", (unsigned int)p_page17_data->instantaneous_cadence);
            printf("Page17:accumulated wheel_period %u\n", (unsigned int)p_page17_data->wheel_period);
            printf("Page17:accumulated_torgue %u\n", (unsigned int)p_page17_data->accumulated_torgue);
#endif // defined(TRACE_DATA_PAGE)                     
            break;

        case BP_PAGE_18:
            // Standard Crank Torque (CT) main data page.
#if defined(TRACE_DATA_PAGE)
            p_page18_data = bp_rx_data_page18_get();

            printf("Page18:update_event_counter %u\n", (unsigned int)p_page18_data->update_event_counter);
            printf("Page18:crank_ticks %u\n", (unsigned int)p_page18_data->crank_ticks);
            printf("Page18:instantaneous_cadence %u\n", (unsigned int)p_page18_data->instantaneous_cadence);
            printf("Page18:accumulated crank_period %u\n", (unsigned int)p_page18_data->crank_period);
            printf("Page18:accumulated_torgue %u\n", (unsigned int)p_page18_data->accumulated_torgue);
#endif // defined(TRACE_DATA_PAGE)
            break;

        case BP_PAGE_32:
            // Standard Crank Torque Frequency (CTF) main data page.
#if defined(TRACE_DATA_PAGE)
            p_page32_data = bp_rx_data_page32_get();

            printf("Page32:event_counter %u\n", (unsigned int)p_page32_data->update_event_counter);
            printf("Page32:slope %u\n", (unsigned int)p_page32_data->slope);
            printf("Page32:time_stamp %u\n", (unsigned int)p_page32_data->time_stamp);
            printf("Page32:torque_ticks_stamp %u\n", (unsigned int)p_page32_data->torque_ticks_stamp);
            printf("Page32:average_cadence %u\n", (unsigned int)p_page32_data->average_cadence);
#endif // defined(TRACE_DATA_PAGE)
            break;

        case COMMON_PAGE_80:
            // Manufacturer's identification common data page.
#if defined(TRACE_DATA_PAGE)
            p_page80_data = bp_rx_data_page80_get();

            printf("Page80:hw_revision %u\n", (unsigned int)p_page80_data->hw_revision);
            printf("Page80:manufacturing_id %u\n", (unsigned int)p_page80_data->manufacturing_id);
            printf("Page80:model_number %u\n", (unsigned int)p_page80_data->model_number);
#endif // defined(TRACE_DATA_PAGE)
            break;

        case COMMON_PAGE_81:
            // Product information common data page.
#if defined(TRACE_DATA_PAGE)
            p_page81_data = bp_rx_data_page81_get();

            printf("Page81:sw_revision %u\n", (unsigned int)p_page81_data->sw_revision);
            printf("Page81:serial_number %u\n", (unsigned int)p_page81_data->serial_number);
#endif // defined(TRACE_DATA_PAGE)
            break;

        default:
            APP_ERROR_HANDLER(p_event_return->param1);
            break;
    }
}